diff --git a/.gitignore b/.gitignore index c9173c8db19..b8c94ae2bd3 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +3rdparty/qpOASES/qpOASES-3.0/ plugin/hrpsysBaseStub.jar idl/*.hh idl/*.cpp diff --git a/.travis.sh b/.travis.sh index 18e146eb617..c6b63717f22 100755 --- a/.travis.sh +++ b/.travis.sh @@ -4,6 +4,7 @@ set -x function error { travis_time_end 31 + mkdir -p ${HOME}/.ros/test_results || echo "OK" find ${HOME}/.ros/test_results -type f -exec echo "=== {} ===" \; -exec\ cat {} \; for file in ${HOME}/.ros/log/rostest-*; do echo "=== $file ==="; cat \$\ @@ -33,6 +34,16 @@ function travis_time_end { set -x } +export DEBIAN_FRONTEND=noninteractive +apt-get update -q=5 +apt-get install -q=5 -y wget sudo git sed +apt-get install -q=5 -y software-properties-common # apt-add-repository +apt-get install -q=5 -y lsb-release +apt-get install -q=5 -y tzdata +lsb_release -a +echo $ROS_DISTRO +echo $DISTRO + travis_time_start mongo_hack # MongoDB hack @@ -47,7 +58,7 @@ travis_time_start setup_ros export CI_SOURCE_PATH=$(pwd) export REPOSITORY_NAME=${PWD##*/} echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" -sudo sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu precise main" > /etc/apt/sources.list.d/ros-latest.list' +sudo -E sh -c 'echo "deb http://packages.ros.org/ros-shadow-fixed/ubuntu ${DISTRO} main" > /etc/apt/sources.list.d/ros-latest.list' wget http://packages.ros.org/ros.key -O - | sudo apt-key add - sudo apt-get update -qq @@ -66,9 +77,9 @@ case $TEST_PACKAGE in travis_time_start install_python #http://askubuntu.com/questions/204510/how-to-install-python-2-5-4 - sudo apt-add-repository -y ppa:fkrull/deadsnakes + sudo apt-add-repository -y ppa:deadsnakes sudo apt-get update -qq - sudo apt-get install -qq -y python2.5 python3.4 + sudo apt-get install -qq -y --force-yes python2.5 python2.7 python3.4 travis_time_end travis_time_start check_python @@ -102,8 +113,8 @@ case $TEST_PACKAGE in stable_rtc) travis_time_start install_openrtm - sudo apt-get install -qq -y omniidl diffstat wget ros-hydro-openrtm-aist - source /opt/ros/hydro/setup.bash + sudo apt-get install -qq -y omniidl diffstat wget ros-${ROS_DISTRO}-openrtm-aist + source /opt/ros/${ROS_DISTRO}/setup.bash ## check stableRTCList sed -i 's@^from@#from@g' python/hrpsys_config.py sed -i 's@^import@#import@' python/hrpsys_config.py @@ -121,8 +132,8 @@ case $TEST_PACKAGE in mkdir stable_idl for idl_file in SequencePlayerService.idl StateHolderService.idl ForwardKinematicsService.idl CollisionDetectorService.idl SoftErrorLimiterService.idl DataLoggerService.idl ExecutionProfileService.idl HRPDataTypes.idl RobotHardwareService.idl ; do wget https://github.com/fkanehiro/hrpsys-base/raw/315.1.9/idl/${idl_file} -O stable_idl/${idl_file} - omniidl -bcxx -I/opt/ros/hydro/include/openrtm-1.1/rtm/idl/ idl/${idl_file} - omniidl -bcxx -I/opt/ros/hydro/include/openrtm-1.1/rtm/idl/ -C stable_idl stable_idl/${idl_file} + omniidl -bcxx -I/opt/ros/${ROS_DISTRO}/include/openrtm-1.1/rtm/idl/ idl/${idl_file} + omniidl -bcxx -I/opt/ros/${ROS_DISTRO}/include/openrtm-1.1/rtm/idl/ -C stable_idl stable_idl/${idl_file} sk_file=$(basename ${idl_file} .idl)SK.cc cat ${sk_file} cat stable_idl/${sk_file} @@ -142,10 +153,10 @@ case $TEST_PACKAGE in travis_time_end travis_time_start install_openhrp3 - sudo apt-get install -qq -y ros-hydro-openhrp3 - source /opt/ros/hydro/setup.bash + sudo apt-get install -qq -y ros-${ROS_DISTRO}-openhrp3 + source /opt/ros/${ROS_DISTRO}/setup.bash if [ "$USE_SRC_OPENHRP3" == true ] ; then - sudo dpkg -r --force-depends ros-hydro-openhrp3 + sudo dpkg -r --force-depends ros-${ROS_DISTRO}-openhrp3 mkdir -p ~/build_openhrp3 cd ~/build_openhrp3 git clone http://github.com/fkanehiro/openhrp3 @@ -179,13 +190,13 @@ case $TEST_PACKAGE in travis_time_start install_$TEST_PACKAGE pkg=$TEST_PACKAGE - sudo apt-get install -qq -y python-wstool ros-hydro-catkin ros-hydro-mk ros-hydro-rostest ros-hydro-rtmbuild ros-hydro-roslint > /dev/null + sudo apt-get install -qq -y python-wstool ros-${ROS_DISTRO}-catkin ros-${ROS_DISTRO}-mk ros-${ROS_DISTRO}-rostest ros-${ROS_DISTRO}-rtmbuild ros-${ROS_DISTRO}-roslint > /dev/null - sudo apt-get install -qq -y ros-hydro-pcl-ros ros-hydro-moveit-commander ros-hydro-rqt-robot-dashboard > /dev/null + sudo apt-get install -qq -y ros-${ROS_DISTRO}-pcl-ros ros-${ROS_DISTRO}-moveit-commander ros-${ROS_DISTRO}-rqt-robot-dashboard > /dev/null - sudo apt-get install -qq -y ros-hydro-$pkg + sudo apt-get install -qq -y ros-${ROS_DISTRO}-$pkg - source /opt/ros/hydro/setup.bash + source /opt/ros/${ROS_DISTRO}/setup.bash travis_time_end travis_time_start setup_catkin_ws @@ -216,9 +227,9 @@ case $TEST_PACKAGE in travis_time_start compile_and_install_downstream - sudo dpkg -r --force-depends ros-hydro-hrpsys + sudo dpkg -r --force-depends ros-${ROS_DISTRO}-hrpsys - catkin_make_isolated --install -j1 -l1 + catkin_make_isolated --install # you need to pretend this is catkin package since you only have hrpsys in catkin_ws export ROS_PACKAGE_PATH=`pwd`/install_isolated/share:`pwd`/install_isolated/stacks:$ROS_PACKAGE_PATH source install_isolated/setup.bash @@ -239,8 +250,8 @@ case $TEST_PACKAGE in wstool set rtmros_hironx http://github.com/start-jsk/rtmros_hironx --git -y wstool set rtmros_nextage http://github.com/tork-a/rtmros_nextage --git -y wstool update - sudo apt-get install -qq -y ros-hydro-urdf - sudo dpkg -r --force-depends ros-hydro-hrpsys + sudo apt-get install -qq -y ros-${ROS_DISTRO}-urdf + sudo dpkg -r --force-depends ros-${ROS_DISTRO}-hrpsys export ROS_LANG_DISABLE=genlisp cd .. @@ -260,7 +271,7 @@ case $TEST_PACKAGE in travis_time_end travis_time_start compile_new_version - catkin_make_isolated -j1 -l1 --install --only-pkg-with-deps `echo $pkg | sed s/-/_/g` | grep -v '^-- \(Up-to-date\|Installing\):' | grep -v 'Generating \(Python\|C++\) code from' | grep -v '^Compiling .*.py ...$' | uniq + catkin_make_isolated --install --only-pkg-with-deps `echo $pkg | sed s/-/_/g` | grep -v '^-- \(Up-to-date\|Installing\):' | grep -v 'Generating \(Python\|C++\) code from' | grep -v '^Compiling .*.py ...$' | uniq rm -fr ./install_isolated/hrpsys/share/hrpsys ./install_isolated/hrpsys/lib/pkgconfig/hrpsys.pc source install_isolated/setup.bash @@ -276,13 +287,19 @@ case $TEST_PACKAGE in # sed -i "1imacro(dummy_macro)\nmessage(\"dummy(\${ARGN})\")\nendmacro()" hrpsys/catkin.cmake sed -i "s@install(DIRECTORY test share@dummy_macro(DIRECTORY test share@" hrpsys/catkin.cmake - sed -i "s@find_package(catkin REQUIRED COMPONENTS rostest mk openrtm_aist openhrp3)@find_package(catkin REQUIRED COMPONENTS rostest mk)\nset(openrtm_aist_PREFIX /opt/ros/hydro/)\nset(openhrp3_PREFIX /opt/ros/hydro/)@" hrpsys/catkin.cmake + sed -i "s@find_package(catkin REQUIRED COMPONENTS rostest mk openrtm_aist openhrp3)@find_package(catkin REQUIRED COMPONENTS rostest mk)\nset(openrtm_aist_PREFIX /opt/ros/${ROS_DISTRO}/)\nset(openhrp3_PREFIX /opt/ros/${ROS_DISTRO}/)@" hrpsys/catkin.cmake cat hrpsys/catkin.cmake sed -i "s@NUM_OF_CPUS = \$(shell grep -c '^processor' /proc/cpuinfo)@NUM_OF_CPUS = 2@" hrpsys/Makefile.hrpsys-base sed -i "s@touch installed@@" hrpsys/Makefile.hrpsys-base cat hrpsys/Makefile.hrpsys-base # use git repository, instead of svn due to googlecode shoutdown - git clone http://github.com/fkanehiro/hrpsys-base --depth 1 -b 315.1.9 ../build_isolated/hrpsys/build/hrpsys-base-source + git clone http://github.com/fkanehiro/hrpsys-base ../build_isolated/hrpsys/build/hrpsys-base-source + (cd ../build_isolated/hrpsys/build/hrpsys-base-source; git checkout 315.1.9) + if [ "${ROS_DISTRO}" != "hydro" ]; then + # tmp fix to build old hrpsys + sudo ln -s /usr/lib/x86_64-linux-gnu/libboost_thread.so /usr/lib/x86_64-linux-gnu/libboost_thread-mt.so + sudo ln -s /usr/lib/x86_64-linux-gnu/libboost_system.so /usr/lib/x86_64-linux-gnu/libboost_system-mt.so + fi # we use latest hrpsys_ocnfig.py for this case, so do not install them sed -i -e 's/\(add_subdirectory(python)\)/#\1/' ../build_isolated/hrpsys/build/hrpsys-base-source/CMakeLists.txt sed -i -e 's/\(add_subdirectory(test)\)/#\1/' ../build_isolated/hrpsys/build/hrpsys-base-source/CMakeLists.txt @@ -311,7 +328,7 @@ case $TEST_PACKAGE in trap 0 ERR need_compile=1 while [ $need_compile != 0 ]; do - catkin_make_isolated -j1 -l1 --merge + catkin_make_isolated --merge need_compile=$? done trap error ERR @@ -322,7 +339,7 @@ case $TEST_PACKAGE in trap 0 ERR need_compile=1 while [ $need_compile != 0 ]; do - catkin_make_isolated -j1 -l1 --install | grep -v '^-- \(Up-to-date\|Installing\):' + catkin_make_isolated --install | grep -v '^-- \(Up-to-date\|Installing\):' need_compile=${PIPESTATUS[0]} done trap error ERR @@ -350,24 +367,26 @@ case $TEST_PACKAGE in rospack profile - # https://github.com/fkanehiro/openhrp3/issues/46 - ls -al /opt/ros/hydro/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController || echo "ok" - sudo mkdir -p /opt/ros/hydro/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController/etc/ || echo "ok" - sudo wget https://raw.githubusercontent.com/fkanehiro/openhrp3/master/sample/controller/SampleController/etc/Sample.pos -O /opt/ros/hydro/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController/etc/Sample.pos || echo "ok" - sudo wget https://raw.githubusercontent.com/fkanehiro/openhrp3/master/sample/controller/SampleController/etc/Sample.vel -O /opt/ros/hydro/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController/etc/Sample.vel || echo "ok" - # https://github.com/start-jsk/rtmros_hironx/issues/287 - if [ -e /opt/ros/hydro/share/hironx_ros_bridge/test/test_hironx_ros_bridge.py ]; then - sudo sed -i "s@test_tf_and_controller@_test_tf_and_controller@" /opt/ros/hydro/share/hironx_ros_bridge/test/test_hironx_ros_bridge.py - fi - #https://github.com/start-jsk/rtmros_hironx/pull/358 - if [ -e /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py ]; then - sudo wget https://raw.githubusercontent.com/k-okada/rtmros_hironx/stop_unfinished_battle/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py -O /opt/ros/hydro/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py - fi - #https://github.com/start-jsk/rtmros_common/commit/51ec26b899f09304705fe0528a068e57b061b9b7 - #https://github.com/start-jsk/rtmros_common/pull/880 - #https://github.com/start-jsk/rtmros_common/pull/879 - if [ -e /opt/ros/hydro/share/hrpsys_ros_bridge/test/test-samplerobot.test ]; then - sudo wget https://raw.githubusercontent.com/start-jsk/rtmros_common/1.3.1/hrpsys_ros_bridge/test/test-samplerobot.test -O /opt/ros/hydro/share/hrpsys_ros_bridge/test/test-samplerobot.test + if [ "${ROS_DISTRO}" == "hydro" ]; then + # https://github.com/fkanehiro/openhrp3/issues/46 + ls -al /opt/ros/${ROS_DISTRO}/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController || echo "ok" + sudo mkdir -p /opt/ros/${ROS_DISTRO}/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController/etc/ || echo "ok" + sudo wget https://raw.githubusercontent.com/fkanehiro/openhrp3/master/sample/controller/SampleController/etc/Sample.pos -O /opt/ros/${ROS_DISTRO}/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController/etc/Sample.pos || echo "ok" + sudo wget https://raw.githubusercontent.com/fkanehiro/openhrp3/master/sample/controller/SampleController/etc/Sample.vel -O /opt/ros/${ROS_DISTRO}/share/openhrp3/share/OpenHRP-3.1/sample/controller/SampleController/etc/Sample.vel || echo "ok" + # https://github.com/start-jsk/rtmros_hironx/issues/287 + if [ -e /opt/ros/${ROS_DISTRO}/share/hironx_ros_bridge/test/test_hironx_ros_bridge.py ]; then + sudo sed -i "s@test_tf_and_controller@_test_tf_and_controller@" /opt/ros/${ROS_DISTRO}/share/hironx_ros_bridge/test/test_hironx_ros_bridge.py + fi + #https://github.com/start-jsk/rtmros_hironx/pull/358 + if [ -e /opt/ros/${ROS_DISTRO}/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py ]; then + sudo wget https://raw.githubusercontent.com/k-okada/rtmros_hironx/stop_unfinished_battle/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py -O /opt/ros/${ROS_DISTRO}/lib/python2.7/dist-packages/hironx_ros_bridge/hironx_client.py + fi + #https://github.com/start-jsk/rtmros_common/commit/51ec26b899f09304705fe0528a068e57b061b9b7 + #https://github.com/start-jsk/rtmros_common/pull/880 + #https://github.com/start-jsk/rtmros_common/pull/879 + if [ -e /opt/ros/${ROS_DISTRO}/share/hrpsys_ros_bridge/test/test-samplerobot.test ]; then + sudo wget https://raw.githubusercontent.com/start-jsk/rtmros_common/1.3.1/hrpsys_ros_bridge/test/test-samplerobot.test -O /opt/ros/${ROS_DISTRO}/share/hrpsys_ros_bridge/test/test-samplerobot.test + fi fi travis_time_end diff --git a/.travis.yml b/.travis.yml index a799907b900..d75b1ae2318 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,11 +1,7 @@ cache: apt -language: - - cpp - - python -python: - - "2.7" -compiler: - - gcc +language: generic +sudo: required +dist: trusty notifications: slack: jsk-robotics:Av7tc8wj3IWkLYvlTzHE7x2g # TEST_TYPE: @@ -16,27 +12,78 @@ env: - secure: "ESSo9YL0lUB0nnlFIL1vV7zpOyZjgTign4IZR735laiylPjOLKW4BkP9JxT5alA9ip72qbpDKI192Ed3sSkWjjQxhM+/HIjtyOy5/F6U5wfvxWudorJeF4zCy4OBD7Bp9JWqucAkRmd5Qe3Djaih69rAK31IFeNhQ2Ebz6RWVGk=" matrix: # test for normal hrpsys users, this will not depends on any ROS-related tools - - TEST_PACKAGE=hrpsys - - TEST_PACKAGE=hrpsys USE_SRC_OPENHRP3=true - - TEST_PACKAGE=hrpsys COMPILE_OPTION='-DROBOT_IOB_VERSION=0' - - TEST_TYPE=python TEST_PACKAGE=hrpsys - - TEST_TYPE=iob TEST_PACKAGE=hrpsys - - TEST_TYPE=stable_rtc TEST_PACKAGE=hrpsys + - ROS_DISTRO=hydro TEST_PACKAGE=hrpsys + # - ROS_DISTRO=hydro TEST_PACKAGE=hrpsys USE_SRC_OPENHRP3=true + # - ROS_DISTRO=hydro TEST_PACKAGE=hrpsys COMPILE_OPTION='-DROBOT_IOB_VERSION=0' + # - ROS_DISTRO=hydro TEST_TYPE=python TEST_PACKAGE=hrpsys + - ROS_DISTRO=hydro TEST_TYPE=iob TEST_PACKAGE=hrpsys + - ROS_DISTRO=hydro TEST_TYPE=stable_rtc TEST_PACKAGE=hrpsys # compile source code with ros-related tools, they are compiled with catkin and test with rostest # Exec USE_SRC_OPENHRP3=true tests in faster orders to make debug of these tests easy. - - TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true - - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true - - TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base USE_SRC_OPENHRP3=true + # - ROS_DISTRO=hydro TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true + # - ROS_DISTRO=hydro TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true + # - ROS_DISTRO=hydro TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base USE_SRC_OPENHRP3=true # USE_SRC_OPENHRP3=false tests - - TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base - - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-base - - TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-tools - - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-tools - - TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-ros-bridge - - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-ros-bridge - - TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge - - TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge -script: source .travis.sh + # - ROS_DISTRO=hydro TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base + # - ROS_DISTRO=hydro TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-base + # - ROS_DISTRO=hydro TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-tools + # - ROS_DISTRO=hydro TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-tools + - ROS_DISTRO=hydro TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-ros-bridge + - ROS_DISTRO=hydro TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-ros-bridge + - ROS_DISTRO=hydro TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge + - ROS_DISTRO=hydro TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge + - ROS_DISTRO=indigo TEST_PACKAGE=hrpsys + - ROS_DISTRO=indigo TEST_PACKAGE=hrpsys USE_SRC_OPENHRP3=true + - ROS_DISTRO=indigo TEST_PACKAGE=hrpsys COMPILE_OPTION='-DROBOT_IOB_VERSION=0' + - ROS_DISTRO=indigo TEST_TYPE=python TEST_PACKAGE=hrpsys + - ROS_DISTRO=indigo TEST_TYPE=iob TEST_PACKAGE=hrpsys + - ROS_DISTRO=indigo TEST_TYPE=stable_rtc TEST_PACKAGE=hrpsys + # compile source code with ros-related tools, they are compiled with catkin and test with rostest + # Exec USE_SRC_OPENHRP3=true tests in faster orders to make debug of these tests easy. + - ROS_DISTRO=indigo TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true + - ROS_DISTRO=indigo TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true + - ROS_DISTRO=indigo TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base USE_SRC_OPENHRP3=true + # USE_SRC_OPENHRP3=false tests + - ROS_DISTRO=indigo TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base + - ROS_DISTRO=indigo TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-base + - ROS_DISTRO=indigo TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-tools + - ROS_DISTRO=indigo TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-tools + - ROS_DISTRO=indigo TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-ros-bridge + - ROS_DISTRO=indigo TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-ros-bridge + - ROS_DISTRO=indigo TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge + - ROS_DISTRO=indigo TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge + - ROS_DISTRO=kinetic TEST_PACKAGE=hrpsys + # - ROS_DISTRO=kinetic TEST_PACKAGE=hrpsys USE_SRC_OPENHRP3=true + # - ROS_DISTRO=kinetic TEST_PACKAGE=hrpsys COMPILE_OPTION='-DROBOT_IOB_VERSION=0' + # - ROS_DISTRO=kinetic TEST_TYPE=python TEST_PACKAGE=hrpsys + - ROS_DISTRO=kinetic TEST_TYPE=iob TEST_PACKAGE=hrpsys + - ROS_DISTRO=kinetic TEST_TYPE=stable_rtc TEST_PACKAGE=hrpsys + # compile source code with ros-related tools, they are compiled with catkin and test with rostest + # Exec USE_SRC_OPENHRP3=true tests in faster orders to make debug of these tests easy. + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge USE_SRC_OPENHRP3=true + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base USE_SRC_OPENHRP3=true + # # USE_SRC_OPENHRP3=false tests + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-base + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-base + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-tools + # - ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-tools + - ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-ros-bridge + - ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-ros-bridge + - ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge + - ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge +matrix: + allow_failures: + - env: ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hrpsys-ros-bridge + - env: ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hrpsys-ros-bridge + - env: ROS_DISTRO=kinetic TEST_TYPE=work_with_downstream TEST_PACKAGE=hironx-ros-bridge + - env: ROS_DISTRO=kinetic TEST_TYPE=work_with_315_1_10 TEST_PACKAGE=hironx-ros-bridge +script: + - case $ROS_DISTRO in "hydro") export DISTRO=precise;; "indigo") export DISTRO=trusty;; "kinetic") export DISTRO=xenial;; esac; + - export DOCKER_IMAGE=ubuntu:$DISTRO + - export CI_SOURCE_PATH=$(pwd) + - docker images + - docker run -v $HOME:$HOME -e CI_SOURCE_PATH -e HOME -e DISTRO -e ROS_DISTRO -e ROS_LOG_DIR -e TEST_TYPE -e TEST_PACKAGE -e USE_SRC_OPENHRP3 -e COMILE_OPTION $DOCKER_IMAGE bash -c 'cd $CI_SOURCE_PATH; source .travis.sh' after_success: - set +x - export GIT_COMMITTER_NAME=$GIT_NAME @@ -49,10 +96,13 @@ after_success: - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then cd doc; fi - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then cp -r ~/build/doc/html/* ./; fi - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git status; fi + - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git checkout --orphan gh-pages-new; fi - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git add -f .; fi - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git commit -m "Build documents from $TRAVIS_COMMIT" . ; fi + - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git branch -D gh-pages; fi + - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git branch -m gh-pages; fi - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git remote -v; fi - - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git push --quiet https://$GH_TOKEN@github.com/$TRAVIS_REPO_SLUG.git gh-pages; fi + - if [ "$TRAVIS_PULL_REQUEST" == "false" -a "$TRAVIS_BRANCH" == "master" -a "$TEST_PACKAGE" == "hrpsys" -a "$TEST_TYPE" == "" ]; then git push -f --quiet https://$GH_TOKEN@github.com/$TRAVIS_REPO_SLUG.git gh-pages; fi diff --git a/3rdparty/qpOASES/CMakeLists.txt b/3rdparty/qpOASES/CMakeLists.txt index 39f6061d219..bed2233f55f 100644 --- a/3rdparty/qpOASES/CMakeLists.txt +++ b/3rdparty/qpOASES/CMakeLists.txt @@ -1,5 +1,5 @@ if (NOT EXISTS ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0) - execute_process(COMMAND svn co https://projects.coin-or.org/svn/qpOASES/stable/3.0 ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0) + execute_process(COMMAND git clone -b stable/3.0 https://github.com/coin-or/qpOASES.git ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0) execute_process(COMMAND sed -i -e "s/qpOASES\ STATIC/qpOASES\ SHARED/g" ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0/CMakeLists.txt) execute_process(COMMAND cmake -E chdir ${CMAKE_CURRENT_BINARY_DIR}/qpOASES-3.0 make) endif() diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 687629c9193..4ef974a81c8 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,1197 @@ Changelog for package hrpsys ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +315.15.0 (2017-10-22) +--------------------- + +Stable RTCs +============= +* SequencePlayer + * Fix bug in CUBICSPLINE mode (`#1201 `_) + * fix bug in CUBICSOLINE mode a[5] -> a5[i] + * Remove offset while playing patterns (`#1191 `_) + * enable to generate continuous motion from motion patterns + * enable to fix a link while playing patterns +* RobotHardware/DataLogger + * Add data port for TimedRobotState2 `#1195 `_ from fkanehiro/add_rstate2_port + * add an output data port for TimedRobotState2 +* RobotHardware + * Add servo on delay (`#1210 `_) + * [RobotHardware] add document on servoOnDelay + * [RobotHardware] add servoOnDelay configuration variable + * Fix bug in getStatus() (`#1197 `_) + * [RobotHardware] fix a bug in getStatus() +* DataLogger + * Add logSplitter (`#1198 `_) + * add logSplitter +* rtm.py (`#1199 `_) + * [rtm.py] add RTcomponent.getProperties() +* hrpEC + * Add missing initialization (`#1209 `_) + * Add missing initialization + * Detect use-after-free of hrpExecutionContext (`#1208 `_) + * Remove C++11 dependency from previous commit + * Detect use-after-free of hrpExecutionContext +* Travis (travis.sh, travis.yaml) + * Update travis to run hydro/indigo/kinetic (`#1194 `_) + * .travis.sh: remove -j1 -l1 to speed up compile + * Update .travis.yml + * run docker without -it + * disable DEBIAN_FRONTEND, install tzdata before other package https://askubuntu.com/questions/909277/avoiding-user-interaction-with-tzdata-when-installing-certbot-in-a-docker-contai + * .travis.sh: old git does not support -b with tag name + * export DISTRO + * skip hydro specific patches + * use DISTRO + * use ROS_DISTRO instead of hydro + * show lsb_release + * .travis.yml: add hydro/indigo/kinetic: test on .travis.yml + * apt-get install python2.7 + * install apt-get software-properties-common + * mkdir tet_results + * install git,wget,sudo,sed + * use docker to run test + +Unstable RTCs +============= + +* ReferenceForceUpdater + * Add is_hold_value flag writing (`#1213 `_) + * [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] Add missing writing of is_hold_value flag + * Update rfu functions (`#1212 `_) + * [idl/ReferenceForceUpdaterService.idl, rtc/ReferenceForceUpdater] Enable to set transition time for RFU + * [idl/ReferenceForceUpdaterService.idl, rtc/ReferenceForceUpdater] Add no-wait version functions +* AccelerationChecker (`#1205 `_) + * remove a newline + * check joint command acceleration only when servo is on +* AutoBalancer (`#1203 `_) + * [rtc/AutoBalancer/AutoBalancer.cpp] Update for MODE_REF_FORCE_RFU_EXT_MOMENT. Separate MODE_REF_FORCE_RFU_EXT_MOMENT from other UseForceMode. +* CameraImageSaver : Add new component (`#1200 `_) + * add bindParameter() + * add a new component, CameraImageSaver +* CameraImageViewer : Support RTC::CameraImage (`#1196 `_) + * [CameraImageViewer] support RTC::CameraImage +* Stabilizer + * Update calculation of foot origin ext moment (`#1203 `_) + * [rtc/Stabilizer/Stabilizer.*] Update calculation of diff_foot_origin_ext_moment not to use ZMP and to use foot moment + * [rtc/Stabilizer/Stabilizer.cpp] Move calculation of actual foot origin coords. This is expected not to change the program behavior. + * Update travis to run hydro/indigo/kinetic (`#1194 `_) + * sample/SampleRobot/samplerobot_stabilizer.py: on hrpsys < 315.5.0 this outputs huge error log message + +* Contributors: Fumio KANEHIRO, Jun Inoue, Kei Okada, Shunichi Nozawa, Yasuhiro Ishiguro + +315.14.0 (2017-08-04) +--------------------- + +Stable RTCs +============= + +* .gitignore (`#1186 `_) + * [.gitignore] add qpoases directory + +* CMakeLists + * Update for compile definitions (`#1179 `_) + * CMakeLists.txt : add " for string REGREX whoen OPENHRP_DEFINITIONS is not set + * Update compile flags (`#1177 `_) + * CMakeLists.txt : remove -O2 from OPENHRP_DEFINITIONS + +* CollisionDetector + * Update debug message (`#1178 `_) + * CollisionDetector.cpp: setupVclipModel() : display size of vclip model verts + * Add service (`#1174 `_) + * add service for collision loop + +* rtm.py + * Fix (`#1183 `_) + * fix classFromString() + * Update behavior (`#1181 `_) + * 関数名を変更 + * readDataPort、writeDataPort関数で毎回コネクタを生成しないようにする + +* SequencePlayer + * Update sample checking (`#1143 `_) + * [sample/SampleRobot/samplerobot_sequence_player.py] Check return value of checkJointAngles of SequencePlayer sample. + * Update for clearOfGroups (`#1141 `_) + * SequencePlayer.cpp: (setTargetPose) send resetJointGroup to m_seq before call playPatternOfGroup + * add test code to check clearOfGroups work when setTargetPose is called + * Fix bug of return value (`#1139 `_) + * [rtc/SequencePlayer/interpolator.h] Fix bug of return value type of interpolator dimension. + +* hrpsys_config.py + * Update debug message (`#1153 `_) + * hrpsys_config.py: display findComps -> (rtcname) + * Update documentation (`#1135 `_) + * [py][doc] Clarify setTargetPoseRelative's wait behavior (addresses `#1121 `_). + `wait` argument is to regulate the commands previously run before `setTargetPoseRelative`, but there are users who thought it stops subsequent commands. + This change hopefully clarifies the difference. + +Unstable RTCs +============= +* PointCloudViewer + * Update checking (`#1145 `_) + * [PointCloudViewer] Validity check seems not to be needed when is_dense param is correctly set + * [PointCloudViewer] Use union rgb member in PointXYZRGB + * [PointCloudViewer] Fix point validity check, isnan should take floating value + * [PointCloudViewer] Fix validity check of color points + * Support coloring (`#1144 `_) + * [PointCloudViewer] Support color pointcloud view + +* Beeper (`#1143 `_) + * [rtc/Beeper/Beeper.cpp] Update sleep time of beep. Do not access beep device during previous beep command is executed. + +* GraspController (`#1160 `_) + * [rtc/GraspController] Add [] for grasp controller instance name and add more information about parsing of joint group setting + +* For fullbody manipulation : AutoBalancer, Stabilizer, ObjectContactTurnaroundDetector, ReferenceForceUpdater + * Fix bug of initialization (`#1160 `_) + * [rtc/ReferenceForceUpdater] Update to use interpolated results. + * [rtc/ReferenceForceUpdater] Output default value for refFootOriginExtMoment if is_active is false. + * [rtc/Stabilizer] Do not calculate actual foot_origin_ext_moment if in the air, because of invalid act_zmp. + * Update for fullbody manip (`#1192 `_) + * [idl/ObjectContactTurnaroundDetectorService.idl, rtc/ObjectContactTurnaroundDetector] Add TOTAL_MOMENT2 to use end-effector moments + * [idl/ObjectContactTurnaroundDetectorService.idl, rtc/ObjectContactTurnaroundDetector, sample/SampleRobot/samplerobot_carry_object.py] Add friction coefficient wrench. Update samples to print it. + * [rtc/AutoBalancer/AutoBalancer.cpp] Set 1.0 as default value for swingsupport time + * [rtc/Stabilizer/Stabilizer.cpp] Update increment of support_time. In some PC, previous version does not work because of optimized out. + * Update for single-leg support (`#1180 `_) + * [rtc/AutoBalancer] Support foot contact manipulation in static balance point calculation + * [python/hrpsys_config.py, rtc/ObjectContactTurnaroundDetector] Use reference contact states in object contact turnaround detector foot origin coords calculation + * [rtc/Stabilizer/ZMPDistributor.h] Add check for ee size. Currently, two feet contact is supported. + * [rtc/AutoBalancer] Check leg_names length for biped force distribution + * [rtc/ObjectContactTurnaroundDetector] Use footorigincoords calculation in OCTD + * Support fullbody manipulation (`#1151 `_) + * [rtc/ReferenceForceUpdater] Add arm name printing to add more information in debug print + * [sample/SampleRobot/samplerobot_carry_object.py] Add Joint Groups setting for startDefaultUnstableControllers + * [sample/SampleRobot/samplerobot_reference_force_updater.py] Update rfu sample (argument update checking and FootOriginExtMoment) + * [python/hrpsys_config.py] Connect FootOriginExtMoment ports for fullbody contact manipulation + * [rtc/AutoBalancer] Add distibution of zmp to ref force/moment. + * [rtc/AutoBalancer] Input ext moment around the foot origin pos and update static balance point calculation for foot force and ext moment. + * [idl/AutoBalancerService.idl,rtc/AutoBalancer] Add MODE_REF_FORCE_RFU_EXT_MOMENT and MODE_REF_FORCE_WITH_FOOT of UseForceMode for fullbody contact manipulation. + * [rtc/ReferenceForceUpdater] Separate functions for update ref forces / ext moment. + * [rtc/ReferenceForceUpdater] Add update for ext moment around the foot origin pos. + * [idl/ReferenceForceUpdaterService.idl, rtc/ReferenceForceUpdater] Add is_hold_value to choose holding current value or not. If is_hold_value is true, do not update value, otherwise, update value. + * [rtc/ReferenceForceUpdater] Add sensor name for eet param. + * [rtc/ReferenceForceUpdater] Enable to set some RFU parameters while active. Separate parameters which can be changed or not while active. + * [rtc/Stabilizer] Add calculation of ext moment around the foot origin position for fullbody manipulation. + * [rt/Stabilizer] Update for st target values considering foot origin coords + +* ReferenceForceUpdater : Fix bug of get/set value (`#1173 `_) + * [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] Fix bug in get value for is_hold_value flag + * [sample/SampleRobot/samplerobot_reference_force_updater.py] Add check for value setting and add comments + +* Stabilizer + * Support n-sided polygon (`#1189 `_) + * [idl/StabilizerService.idl] add comment about the order of eefm_support_polygon_vertices_sequence + * [Stabilizer.cpp, Stabilizer.cpp] add vertices with margin for fall detection + * [ZMPDistributor.h] remove unused function + * [ZMPDistributor.h] fix bug on calcProjectedPoint + * [Stabilizer.*, ZMPDistributor.h] calculate convex hull from n-sided polygon's vertices + * Fix bug of initialization (`#1190 `_) + * [rtc/Stabilizer/Stabilizer.cpp] Fix bug of support_time. Initialize support_time and limit max support_time (too large support_time). Add debug print. + * Support ZMP truncate (`#1158 `_) + * [idl/StabilizerService.idl, rtc/Stabilizer/Stabilizer.cpp,Stabilizer.h,ZMPDistributor.h] add option of zmp truncation + * [rtc/Stabilizer/ZMPDistributor.h] Add func to calculate nearest point of foot support polygon + +* AutoBalancer + * Update ref force balancing (`#1188 `_) + * [idl/AutoBalancerService.idl,python/hrpsys_config.py,rtc/AutoBalancer,rtc/ReferenceForceUpdater] Support walking while MODE_REF_FORCE_RFU_EXT_MOMENT by using is_hold_value flag and move base offset + * [idl/AutoBalancerService.idl, rtc/AutoBalancer] Add parameters to specify link name and offset point for additional force, such as torso. + * Update footstep limitation (`#1185 `_) + * [rtc/AutoBalancer/GaitGenerator.h, idl/AutoBalancerService.idl] Update documentations. Add new explanations and fix incorrect explanations. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add sample functions for stride limitation + * [rtc/AutoBalancer/GaitGenerator.cpp] Use inside limitation in footstep calculation. + * [idl/AutoBalancerService.idl, rtc/AutoBalancer] Enable to set stride_inside_y and stride_inside_th as stride_parameter + * [rtc/AutoBalancer/AutoBalancer.cpp, Gaitgenerator] Rename stride_y and stride_theta -> stride_outside_y and stride_outside_theta + * Fix bug of limiting (`#1162 `_) + * [rtc/AutoBalancer/GaitGenerator, sample/SampleRobot/samplerobot_auto_balancer.py] Fix foot step limitation in case of offset vel is specified. Fix testing code. + * Fix bug of initialization (`#1157 `_) + * [rtc/AutoBalancer/GaitGenerator.h] set state at first double support phase and last double support phase to modify bug of no double support walk without reducing swing count. + * Fix bug of GaitGenerator memory access (`#1159 `_) + * [rtc/AutoBalancer/GaitGenerator] Fix invalid access for step_count_list (https://github.com/fkanehiro/hrpsys-base/issues/1154) + * [rtc/AutoBalancer/GaitGenerator] Remove unused argument in update_refzmp. + * Separate simplefullbodyiksolver (`#1150 `_) + * move operator<< overload into .cpp + * separate into SimpleFullbodyInverseKinematicsSolver.h + * Support footstep modification in goVelocity (`#1147 `_) + * [GaitGenerator.*] support goVelocity in modifying footsteps + * [GaitGenerator.cpp] remove unused variable + * Update preview queue in GaitGenerator (`#1140 `_) + * [GaitGenerator.cpp] overwrite refzmp queue only when is_emergency_walking + * [GaitGenerator.cpp, PreviewController.h] overwrite preview queue without clearing queue + * Update testing and plotting of GaitGenerator (`#1143 `_) + * [rtc/AutoBalancer/testGaitGenerator.cpp] Update comments and print message. Add TODO memo. + * [rtc/AutoBalancer/testGaitGenerator.cpp, GaitGenerator.*] Add check for toe heel angle and zmp offset transigion. Use eps_eq instead of calling fabs directly. + * [rtc/AutoBalancer/CMakeLists.txt, GaitGenerator.h, testGaitGenerator.cpp] Add check for step times. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add error checker class and add start/end error check between COGxy and REFZMPxy + * [rtc/AutoBalancer/CMakeLists.txt] Enable test6 checking in make test + * [rtc/AutoBalancer/testGaitGenerator.cpp] Update comment and use fflush to confirm file writing. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add value discontinuous checking for state values. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Update test for single step and z change (test6, test5) to remove discontinuous results. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Fix empty range of gnuplotting (force set range). + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add test_doc_string, add test name to file name, and use sleep for use-graph-append. + * Fix bug of discontinuous trajectory (`#1139 `_) + * [rtc/AutoBalancer/testGaitGenerator.cpp] Update comments and documentation. Add appepnding of graphs. + * [rtc/AutoBalancer/CMakeLists.txt] Call tests for test17 and test18 in 'make test' (govelocity tests) + * [rtc/AutoBalancer/GaitGenerator.*] Update rot eps for mid_coords functions. + * [rtc/ImpedanceController/RatsMatrix.*] Add eps argument for mid_rot and mid_coords + * [rtc/AutoBalancer/testGaitGenerator.cpp] Increase velocity plotting + * [rtc/AutoBalancer/GaitGenerator.*] Enable to update the target value of foot midcoords (for example, foot step modification for updown and rotation). This bug is originally reported by @kyawawa. + * [rtc/AutoBalancer/GaitGenerator.cpp] Interpolate difference from src to dst in swing_foot_rot_interpolator + * [rtc/AutoBalancer/GaitGenerator.h] Calculate dst and src swing coordinates midcoords for all limbs at first. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add plot for swing support mid coords + * [rtc/AutoBalancer/testGaitGenerator.cpp] Separate plot files. This will not change behavior. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add argument for default-double-support-ratio-swing-[after/before] + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add rotation velocity change for test18 govelocity test + * [rtc/AutoBalancer/GaitGenerator.*] Enable to update the target value of swing foot rot (for example, foot step modification for rotation) + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add test for changing govelocity velocities + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add test for goVelocity mode. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Separate plot and walk pattern generation parts as functions. This will not change the program behavior. + + +* Contributors: Fumio KANEHIRO, Iori Kumagai, Isaac I.Y. Saito, Juntaro Tamura, Kei Okada, Nobuhiko Miyamoto, Noriaki Takasugi, Shunichi Nozawa, Yuta Kojio, Yasuhiro Ishiguro, Yisha + +315.13.0 (2017-05-19) +--------------------- + +Stable RTCs +============= + +* Update for beeping of SoftErrorLimiter and CollisionDetector (`#1124 `_) + * [rtc/SoftErrorLimiter/SoftErrorLimiter.cpp,rtc/CollisionDetector/CollisionDetector.cpp] Call quit_beep when is_beep_port_connected is true, because beeping is not required in these RTCs and deregated to Beeper RTC in this case. + +* Update documentations of python interfaces for stable rtcs (`#1120 `_) + * [doc][py] Explain setWrenches method (thanks to https://github.com/fkanehiro/hrpsys-base/issues/1131). + Similar info should be added to IDL level (this Python file is only the end-user API so the number of audience can be minimum). + * [py][doc] Add how to set ref\_{force, moment} for 315.2.0 <= version. + * [doc][py] More elaboration on startImpedance. + * [py] Elaborate the "tm" arg. + AFAI understood from looking at [SequencePlayer/SequencePlayer.cpp](https://github.com/fkanehiro/hrpsys-base/blob/350a3bcdeafd39d82a44c2dbde6a279adb6d88eb/rtc/SequencePlayer/SequencePlayer.cpp#L547)'s implementation, time taken for planning and other processes are not considered for "`tm`" argument. + * [doc][py] Elaborate playPattern* methods. + +* hrpsys-simulator + * set default background color of hrpsys-viewer + +* Update CMake for VTK (`#1123 `_) + * Precise the version of VTK + There seems to be an incompatibility between the version 5.8 and the version 7 + +Unstable RTCs +============= +* Update Unstable RTC sample, readme, and images (`#1126 `_, `#1127 `_) + * [sample/SampleRobot/README.md] Fix path of images of README + * [sample/SampleRobot/README.md, img] Add image and update link for hrpsys sample README + * [sample/SampleRobot/samplerobot_terrain_walk.py] Update terrain walk samples + * [sample/SampleRobot/SampleRobot.DRCTestbed.xml] Update DRC testbed xml to add floor. + * [sample/README.md, SampleRobot/README.md] Update README to fix indent and alignment. + +* Update Stabilizer (`#1125 `_, `#1128 `_, `#1137 `_) + * Update Stabilizer-related codes and samples (`#1125 `_) + * [sample/SampleRobot/samplerobot_stabilizer.py] Add new sample for st for root rot change and mimic rough terrain. + * [rtc/PDcontroller/PDcontroller.cpp] Print PD gain when loading + * [rtc/Stabilizer/Stabilizer.*] Remove unused parameters. + * Update damping control in horizontal direction (`#1128 `_) + * [Stabilizer] Enable to change horizontal swing damping gain when detect large force/moment + * IK loop and base rot control (`#1137 `_) + * [rtc/Stabilizer/Stabilizer.* idl/StabilizerService.idl] Enable to set ik_loop_count in st + * [rtc/Stabilizer/ZMPDistributor.h, Stabilizer.cpp] Reduce lines of printing of stabilizer. + * [idl/StabilizerService.idl, rtc/Stabilizer] Enable to set Limit of compensation for difference between ref-act root rot [rad]. + +* Add new functionality for walking (`#1129 `_, `#1134 `_) + * Modify footsteps to keep balance (`#1129 `_) + * [AutoBalancerService.idl, StabilizerService.idl, hrpsys_config.py, AutoBalancer.*, AutoBalancer.*, Stabilizer.*] modify footsteps based on CP + * [AutoBalancerService.idl, AutoBalancer.cpp, GaitGenerator.*, sample/SampleRobot/samplerobot_auto_balancer.py] use different parameter when generating footsteps with circle type + * Change base height to avoid from leg stretching (`#1134 `_) + * [AutoBalancerService.idl, AutoBalancer.*] avoid limb stretch by changing root height + * [Stabilizer.cpp] smoothly change root height when switching use_limb_stretch_avoidance + +* Update python function to start all default controllers (`#1130 `_) + * [python/hrpsys_config.py] Support non-legged robot and leg-only robot in startDefaultUnstableControllers and stopDefaultUnstableControllers + +* Add new functionality to remove force sensor offset (`#1132 `_, `#1133 `_) + * [idl/RemoveForceSensorLinkOffsetService.idl, rtc/RemoveForceSensorLinkOffset, python/hrpsys_config.py, sample/SampleRobot/samplerobot_remove_force_offset.py] Add argument for duration of calibration. Set 8.0[s] as default value in python code + * [python/hrpsys_config.py,sample/SampleRobot/samplerobot_remove_force_offset.py] Add sample and python interface for removing force sensor offset. + * [idl/RemoveForceSensorLinkOffsetService, rtc/RemoveForceSensorLinkOffset] Add function to remove force sensor offset in RMFO RTC. + * [rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt] Add documentation abount two types of offsets. + +* Contributors: Fumio KANEHIRO, Isaac I.Y. Saito, Mehdi Benallegue, Shunichi Nozawa, Tatsuya Ishikawa, YutaKojio + +315.12.1 (2017-04-04) +--------------------- + +Stable RTCs +============= + +* Fix bug of https://github.com/fkanehiro/hrpsys-base/pull/1103 (`#1114 `_) + * [CMakeLists.txt] use separate_arguments to set semicolon to variable. + +Unstable RTCs +============= + +* Update for 3rdparty (`#1115 `_) + * [3rdparty/qpOASES/CMakeLists.txt] trust server of QPOASES + +* Add ApproximateVoxelGridFilter (`#1117 `_) + * replace VoxelGridFilter with ApproximateVoxelGridFilter + * add a new RT component, ApproximateVoxelGridFilter + +* Update for ApproximateVoxelGridFilter + * copy timestamp from input cloud to output cloud + * add debug mode and pass through mode + +* Add PointCloudViewer (`#1116 `_) + * add a new RT component, PointCloudViewer + +* Update for VoxelGridFilter + * copy is_dense attribute from input point cloud + * add debugLevel property and support pass-through mode + +* Update Stabilizer in-the-air behavior (`#1090 `_) + * [Stabilizer] Set default detection time whether robot is in air to zero + * [Stabilizer] Set detection time whether robot is in air + * [Stabilizer] Rename variables + +* Rename and separate functions (`#1111 `_) + * Stabilizer + * [rtc/Stabilizer/Stabilizer.*] Rename contact_states -> ref_contact_states. Rename isContact -> act_contact_states + * AutoBalancer + * [rtc/AutoBalancer/AutoBalancer.*] Separate solving IK codes as class. Currently, transition and SBP calculation is not separated. (This is expected not to change behavior). + * [rtc/AutoBalancer/AutoBalancer.cpp] Output and set OutPort only for legged robots. + * [rtc/AutoBalancer/AutoBalancer.*] Make startWalking without ABC deprecated and fix position of calling of stopWalking and zmp_weight_mzp interpolation. + * [rtc/AutoBalancer/AutoBalancer.*] Remove unnecessary member vaiable (target_end_coords) + * [rtc/AutoBalancer/AutoBalancer*] Separate codes from IK parts. (This commit is expected not to change the behavior) + * [rtc/AutoBalancer/AutoBalancer.*] Include target EE coords calculation in separated functions. (This commit is expected not to change the behavior). + * [rtc/AutoBalancer/AutoBalancer.*] Separate procedures in getTargetParameters as fucntions. (This commit is expected not to change the behavior). + * [rtc/AutoBalancer/AutoBalancer.*] Fix order of transition code. Separate functions to calculate output from ABC. (This commit is expected not to change the behavior). + * [rtc/AutoBalancer/AutoBalancer.cpp] Fix parameter setting section in non-GaitGenerator parts. Fix index of default_zmp_offsets usage. + * [rtc/AutoBalancer/AutoBalancer, GaitGenerator] Separate and move codes from AutoBalancer to GaitGenrator, in order to get GaitGenerator results in AutoBalancer. (This commit is expected not to change the behavior) + * [rtc/AutoBalancer/AutoBalancer.cpp] Use target EE pos and rot instead of target link origin pos and rot from SequencePlayer target. (This commit is expected not to change the behavior) + * ImpedanceController + * [rtc/ImpedanceController/ImpedanceController.*] Separate functions, add comments, and fix order and calculation of calcForceMoment. (This commit is expected not to change the behavior) + * Update samples for unstable RTCs + * [launch/sample4legrobot.launch,sample6dofrobot.launch,samplespecialjointrobot.launch] Update conf_file setting for launch samples + * [sample/SampleRobot/samplerobot_impedance_controller.py] Separate tests as functions and improve execution time and result checking. + +* AutoBalancer update by adding inverse dynamics function (`#1110 `_) + * refactor IIRFilter::setParameterAsBiquadButterworth -> IIRFilter::setParameterAsBiquad + * sqrt -> std::sqrt one more + * sqrt -> std::sqrt + * seperate InverseDynamics calculation sequence + * tan -> std::tan + * Revert "remove unused dependency from AutoBalancer/CMakeLists.txt" + This reverts commit 933edb0a1d05b7104e22eccc96f5639cedd607b4. + * remove unused dependency from AutoBalancer/CMakeLists.txt + * Revert "move InverseDynamics from ABC to IC/JointPathEx" + This reverts commit 9b2a76ea3d6e0f871b4460002ded668649711a19. + * move InverseDynamics from ABC to IC/JointPathEx + * move InverseDynamics from ABC to IC/JointPathEx + * merge from master + * Add inverse dynamics function + +* Contributors: Fumio KANEHIRO, Masaki Murooka, Ryo KOYAMA, Shunichi Nozawa, Tatsuya Ishikawa, Yasuhiro Ishiguro + +315.12.0 (2017-03-08) +--------------------- + +Stable RTCs +============= + +* Do not store history of gh-pages + * Do not store history of gh-pages (`#1107 `_) + * .travis.yml : do not keep history in gh-pages + * Fix git push for gh-pages (`#1108 `_) + * fix `#1107 `_: need to push with -f + +* Fix bug of cmake for latest catkin (`#1103 `_) + * fix https://github.com/start-jsk/rtmros_common/pull/998, for latest catkin + +* Update maintainer and docs (`#1100 `_) + * Update maintainer + * [doc] Clarify workspace for get*Pose methods. [doc] Add missing rtype for setJoint* methods. + + +Unstable RTCs +============= + +* CameraImageViewer OpenNIGrabber + * Update for depth image(`#1106 `_) + * resolve conflicts + * enable output both of image and point cloud + * update document + * add a configuration variable depthBits to CameraImageViewer + * support depth image (not tested yet) + * support CF_DEPTH (not tested yet) + * add CF_RGB to ColorFormat + * Update for OpenNIGrabber (`#1096 `_) + * flip Y and Z coordinates + * enable to change execution period + * set timestamps to output data + * set true to is_dense + +* Stabilizer + * Bug fix of transition among stop, start, ground, and air mode (`#1104 `_) + * [rtc/Stabilizer/Stabilizer.cpp] Fix bug of st transition related with air and ground (https://github.com/fkanehiro/hrpsys-base/issues/1098, https://github.com/fkanehiro/hrpsys-base/pull/1102) + * [sample/SampleRobot/samplerobot_stabilizer.py] Add test code to check ST transition problem (https://github.com/fkanehiro/hrpsys-base/issues/1098, https://github.com/fkanehiro/hrpsys-base/pull/1102). + * Bug fix of stop/start transition (`#1102 `_) + * [rtc/Stabilizer/Stabilizer.cpp] Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST) because the coordinates for ref_cog differs among st algorithms. + * [rtc/Stabilizer/Stabilizer.cpp] Wait for MODE transition regardless of whether the client program is mode changer or not (https://github.com/fkanehiro/hrpsys-base/issues/1098) + * Update for st control low (`#1099 `_) + * [rtc/Stabilizer/Stabilizer.*] Enable to update is_feedback_control_enable flag while MODE_ST + * [rtc/Stabilizer/ZMPDistributor.h] Use 6dof total wrench in ZMPdistribution in EEFMQPCOP2 + +* ObjectContactTurnaroundDetector (Add new RTC separated from ImpedanceController) (`#1101 `_) + * [idl/ImpedanceControllerService.idl,rtc/ImpedanceController] Remove ObjectContactTurnaroundDetector from ImpedanceController. + * [sample/SampleRobot/samplerobot_carry_object.py, samplerobot_impedancecontroller.py] Use OCTD RTC instead of ImpedanceController + * [python/hrpsys_config.py,launch/samplerobot.launch,doc] Update hrpsyspy, launch, and doc for ObjectContactTurnaroundDetector RTC + * [rtc/CMakeLists.txt,idl/CMakeLists.txt,rtc/ObjectContactTurnaroundDetector,idl/ObjectContactTurnaroundDetectorService.idl] Add ObjectContactTurnaroundDetector RTC for object manipulation separated from ImpedanceController + +* TorqueFilter/IIRFilter (`#1097 `_) + * [rtc/TorqueFilter] Add comments for IIR Filter implementation + * [rtc/TorqueFilter] Fix reset function to output initial_value + +* ImpedanceController (`#1099 `_) + * [rtc/ImpedanceController/ImpedanceController.cpp,h,ObjectTurnaroundDetector.h] Add port for otd data (mode, wrench values) + * [rtc/ImpedanceController/ImpedanceController.cpp] Add calcForwardKinematics for state calculation. + +* AutoBalancer (`#1099 `_) + * [rtc/AutoBalancer/AutoBalancer.cpp] Transition m_limbCOPOffset in ABC (in MODE_IDLE, set to 0). + * [rtc/AutoBalancer/AutoBalancer.cpp] Use setGoal instead of go for autobalancer transition. + +* KalmanFilter (`#1099 `_) + * [rtc/KalmanFilter/CMakeLists.txt, testKFilter.cpp] Add test code for KFilter class. + * [rtc/KalmanFilter/KalmanFilter.cpp] Consider sensor offset for gyro value. + +* Contributors: Fumio KANEHIRO, Isaac I.Y. Saito, Kei Okada, Shunichi Nozawa, Iori Kumagai + +315.11.0 (2017-02-17) +--------------------- + +Stable RTCs +============= + +* supports OpenRTM-aist 1.2.0 (`#1076 `_ from fkanehiro/openrtm_1.2.0) + +* add ctype and time to Hrpsys.h for QNX (`#1085 `_) + + * add include time.h in lib/util/Hrpsys.h for QNX + * include ctype.h in lib/util/Hrpsys.h for QNX + +* Documentation + + * idl/SequencePlayerService.idl: add suppored version in @brief (`#1092 `_) + * idl/SequencePlayerService.idl: Fix documentation of setJointAngleSequenceFull (fix typoes and unit systems). (`#1067 `_) + +* DataLogger + + * rtc/DataLogger/DataLogger.cpp: add log_precision (configuration variable) (`#1081 `_) + +* RobotHardware + + * [rtc/RobotHardware/robot.cpp] Fix bugs in setServoGainPercentage condition (`#1074 `_) + +* Add beep trylock, print PDgain, print time in EC debug (`#1060 `_) + + * [rtc/RobotHardware/robot.cpp] Print loaded PD gain. + * [ec/hrpEC/hrpEC-common.cpp] Print gettimeofday time in ENABLE_DEBUG_PRINT + * [rtc/Beeper/Beeper.cpp] Use mutex trylock not to block realtime thread + +* lib/util/PortHandler.cpp : add VisionSensorPortHandler intrinsic paramter to TimedCamera (`#1091 `_ from k-okada/add_camera_info) + +* [rtm.py] try to re-connecting to name server, when failed (`#1083 `_) +* [hrpsys_config.py] Add clearJointAngles* methods. (`#1064 `_) +* [hrpsys_config.py] set HrpsysConfigurator as object class (`#1048 `_) + +Unstable RTCs +============= + +* fix time stamp of warning messages ( `#1089 `_) + + * [Stabilizer] reduce number of print messages + * [AutoBalancer, CollisionDetector, EmergencyStopper, SoftErrorLimitter, Stabilizer, ReferenceForceUpdater] fix time stamp of warning messages +* [AutoBalancer, CollisionDetector, EmergencyStopper, SoftErrorLimitter, Stabilizer] add time stamp to warning messages (`#1087 `_) + +* PCDLoader + + * rtc/PCDLoader/PCDLoader.cpp: support XYZRGB in PCDLoader (`#1093 `_) + +* Stabilizer + + * Fix st damping compensation bug. Enable yaw rotation. (`#1082 `_) + + * [rtc/Stabilizer/Stabilizer.cpp] Fix st damping compensation bug. Enable yaw rotation. + + * Update st debug messages ( `#1075 `_) + + * [sample/SampleRobot/samplerobot_stabilizer.py] Add printing of actual base pos during st testing. + * [rtc/Stabilizer/ZMPDistributor.h, Stabilizer.cpp] Reduce lines of st debug message in setParameter (total information is same). + + * Change root link height depending on limb length (`#1069 `_) + + * [StabilizerService.idl, Stabilizer.*] rename variable and add function for avoiding limb stretch + * [Stabilizer.cpp] change root link height on world frame not root link frame + * [StabilizerService.idl, Stabilizer.*] add idl for changing root link height + * [Stabilizer.*] change root link height depending on limb length + + * do not distribute moment to swing leg (`#1068 `_) + + * [Stabilizer.cpp, ZMPDistributor.h] do not distribute moment to swing leg + + * [AutoBalancer, Stabilizer] add warning message (`#1066 `_) + * Update st swing modif (`#1062 `_) + + * [rtc/Stabilizer/CMakeLists.txt] Fix build dependency not to depends on Stabilizer codes. + * [rtc/Stabilizer/Stabilizer.cpp] Use Eigen cwise functions for swing ee modif calculation. + * [rtc/Stabilizer/Stabilizer.cpp,h] Move vectors for swing ee modif to stikp. + * [rtc/Stabilizer/Stabilizer.cpp] Enable retrieving of swing ee modif during support phase. + * [rtc/Stabilizer/Stabilizer.cpp,h] Separate swing foot modif function and re-organize calcEEForceMomentControl + * [rtc/Stabilizer/Stabilizer.cpp,h] Update swing ee modification to use current state. + + * Initialize st eefm_ee_forcemoment_distribution_weight parameter. (`#1059 `_) + + * [rtc/Stabilizer/Stabilizer.cpp] Initialize st eefm_ee_forcemoment_distribution_weight parameter. + + * Use damping control in swing phase (`#1058 `_) + + * [StabilizerService.idl, Stabilizer.*] use swing damping only when exceeding threthold + * [StabilizerService.idl, Stabilizer.*] use damping control in swing phase + +* AutoBalancer + + * Add and update test for sync sh baseTform (`#1065 `_) + + * [rtc/AutoBalancer/AutoBalancer.cpp] Set fix_leg_coords according to basePos and rpy from StateHolder during MODE_IDLE. + * [sample/SampleRobot/samplerobot_auto_balancer.py] Add and update test for sync sh baseTform + + * Enable auto toeheel (`#1056 `_) + + * [idl/AutoBalancerService.idl, rtc/AutoBalancer/AutoBalancer.cpp] Enable to set use_toe_heel_auto_set, toe,heel_check_thre from idl. + * [rtc/AutoBalancer/*GaitGenerator.*] Fix typo (double -> bool) and add flag whether use auto toe heel set or not. Update test to use use_toe_heel_auto_set flag. + * [rtc/AutoBalancer/GaitGenerator.h] Add toe_heel_type_checker to determin autonomusly whether toe or heel is used. + * [rtc/AutoBalancer/GaitGenerator.*] Separate calc swing support leg steps as functions + * [rtc/AutoBalancer/GaitGenerator.cpp] Use first_xx and second_xx instead of toe_xx and heel_xx. Renaming and fix transition. + + * Update toeheel and st (`#1054 `_) + + * [rtc/Stabilizer/testZMPDistributor.cpp, CMakeLists.txt] Update testZMPDistributor to switch distribute algorithm and add cmake test for them. + * [sample/SampleRobot/samplerobot_stabilizer.py] Add test for st+toe heel usage + * [python/hrpsys_config.py, rtc/Autobalancer, rtc/Stabilizer] Add and connect ports to send toe heel ratio between AutoBalancer and Stabilizer. Add weighting matrix update based on toe heel ratio. + * [rtc/AutoBalancer/testGaitGenerator.cpp, GaitGenerator.h] Add functions to obtain toe heel ratio and add test for it. Update comment in codes. + + * Fix toe heel phase count of refzmp_generator (`#1053 `_) + + * [rtc/AutoBalancer/CMakeLists.txt] Enable cmake test for testGaitGenerator test16 + * [rtc/AutoBalancer/GaitGenerator.*] Fix bug of toe heel phase. Make independent toe heel phase for leg_coords_generator and refzmp_generator. Use thp instead of thp_ptr->. + * [rtc/AutoBalancer/testGaitGenerator.cpp] Update zmp diff check thre (10mm->20mm). Update test16. Update step time. Add default zmp offsets. + * [rtc/AutoBalancer/GaitGenerator.h] Use NUM_TH_PHASES directly. + + * Update abc posik ggtest (`#1052 `_) + + * [rtc/AutoBalancer/testGaitGenerator.cpp, CMakeLists.txt] Add test for setFootStepWithParam related functions. CMake test is disabled currently. + * [rtc/AutoBalancer/AutoBalancer.cpp] Update default value for pos_ik_thre according to hrp2 reset manip pose. 0.1[mm] -> 0.5 [mm]. + +* sample + + * Update SampleRobot xml project file (`#1086 `_) + + * [sample/SampleRobot/samplerobot_auto_balancer.py] Omit demoStandingPosResetting because we use floor5 and we do not need to worry about falling down from the floor. + * [sample/SampleRobot/samplerobot_auto_balancer.py] Add print message for all autobalancer sample time. + * [sample/SampleRobot/README.md,SampleRobot.*.xml.in] Use floor5 instead of longfloor + * [sample/SampleRobot/SampleRobot*.xml.in] Update to sample's xml.in files using latest openhrp-project-generator + + * [sample/SampleRobot/README.md] Add readme to generate sample's xml.in files + * Update st tests (`#1061 `_) + + * [sample/SampleRobot/README.md] Add command documentation to generate xml + * [rtc/AutoBalancer/GaitGenerator.h] Add toe usage for down-stair walking. + * [sample/SampleRobot/samplerobot_stabilizer.py] Update st tests. Add stair test. Re-organize all tests. Use swing damping mode. + * [sample/SampleRobot/SampleRobot.torque.xml.in] Add box for stair walking tests. + +* IIRFilter.cpp + + * [rtc/TorqueFilter/IIRFilter.cpp] Fix problem of resetting coefficient value in setParameter (`#1077 `_) + * [TorqueFilter, IIRFilter.h] add getParameter and reset method (`#1050 `_) + +* AccelerationFilter + + * add AccelerationFilter (`#1051 `_) + * [AccelerationFilter] fix compile AccelerationFilterService.idl + * [AccelerationFilter] add AccelerationFilter to hrpsys_config.py + * [AccelerationFilter] add Acceleration Filter + +* util/SelfCollisionChecker/main.cpp + + * enables to visualize motion with -olv option (`#1055 `_) + +* JpegDetector/JpegEncoder/RGB2Gray/VideoCapture + + * Add opencv missing header and namespace for OpenCV3 (`#1049 `_) + +* Contributors: Fumio KANEHIRO, Isaac Saito, Kei Okada, Shunichi Nozawa, Tatsuya Ishikawa, Yohei Kakiuchi, Yuta Kojio, Iori Yanokura, Yasuhiro Ishiguro + +315.10.1 (2016-10-08) +--------------------- + +Stable RTCs +============= + +* [python/hrpsys_config.py, sample/SampleRobot/*.py] Fix bug of hrpsys version checking. Use StrictVersion for version checking. (`#1044 `_) + + * Closes https://github.com/tork-a/rtmros_nextage/issues/260 + +Unstable RTCs +============= + +* IIRFilter + + * Add setParameter, passFilter methods (`#1046 `_) + * [IIRFilter] remove warning message + * [IIRFilter] add test code for IIRFilter to testIIRFilter + * [IIRFilter] fix indent on IIRFilter.h IIRFilter.cpp + * [IIRFilter] add new method for using IIRFilter + +* AutoBalancer + + * [rtc/AutoBalancer/GaitGenerator.cpp] fix bug of emergency stop (`#1045 `_) + + * Add GaitGenerator Sample codes (`#1043 `_) + + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add test for stairdown + * [rtc/AutoBalancer/GaitGenerator.h] Add print message for swing_trajectory_time_offset_xy2z + + * Update swing trajectory xy and z convergence (`#1042 `_) + + * [rtc/AutoBalancer/testGaitGenerator.cpp] Add --swing-trajectory-time-offset-xy2z option for testGaitGenerator + * [idl/AutoBalancerService.idl, rtc/AutoBalancer/AutoBalancer.cpp,GaitGenerator.h] Add swing_trajectory_time_offset_xy2z for time between Z convergence time and XY convergence time. 0 by default, which does not change default behavior. + * [rtc/AutoBalancer/GaitGenerator.h] Separate xy interpolation and z interpolation + * [rtc/AutoBalancer/GaitGenerator.h] Use int for if check + * [rtc/AutoBalancer/GaitGenerator.h] Return final distance antecedent path ratio + * [rtc/AutoBalancer/GaitGenerator.h] Separate calc function and interpolate function for antecedent path + * [rtc/AutoBalancer/GaitGenerator.h] Define foot hoffarbib_interpolation as double type interpolation + * [rtc/AutoBalancer/GaitGenerator.h] Define interpolate_antecedent_path functions as const member functions. + +* OpenNIGrabber + + * Changes point type for depth_and_color (`#1041 `_) + +* Contributors: Fumio Kanehiro, Shunichi Nozawa, Tatsuya Ishikawa, Yohei Kakiuchi + +315.10.0 (2016-09-13) +--------------------- + +Stable RTCs +============= + +* SequencePlayer + + * [rtc/SequencePlayer/timeUtil.cpp] get_tick(): add support for ARM_ARCH_7A and AARCH64EL (`#1018 `_ ) + +* RobotHardware + + * [rtc/RobotHardware/robot.*] Check read_power_state flags to servo off. If power OFF is detected while servo ON, servo OFF all and write EMG_POWER_OFF. Do not used by default. (`#1036 `_) + + * [rtc/RobotHardware/RobotHardware.cpp] checks if model path is given before trying to load(`#1001 `_) + + * doesn't return RTC_ERROR to pass tests + * returns RTC_ERROR is the model path is not given instead of calling abort() + * checks if model path is given before trying to load + + * Use robothardware with simulation(`#995 `_) + + * [RobotHardware] add accessor to m_robot + * [RobotHardware] base time is taken by a virtual method + +* CollisionDetector + + * [CollisionDetector] fix warning message while servo off (`#1023 `_) + + * get out from initial collision state (`#1015 `_) + + * [CollisionDetector] update test code for collision detector + * [CollisionDetector] fix, get out from initial collision state + * add test for CollisionDetector on initial collision state + + * [CollisionDetector] Fix problem when set collision_loop (`#993 `_, `#990 `_) + + * add version check to test-collision-loop + * [CollisionDetector] fix behavior when using collision_loop + * add version check to samplerobot_collision_detector.py + * add test for collision_loop on CollisionDetector + +* fixes warnings detected by -Wsign-compare (`#1039 `_) +* fixes warnings detected by -Wreorder (`#1038 `_) +* Support clang (`#1037 `_) +* [ec/hrpEC/hrpEC-common.cpp] supports trunk version of OpenRTM-aist (https://github.com/fkanehiro/hrpsys-base/commit/efce7d47dc3723c868b66bf6205f93bab99b1537) +* [lib/util, rtc/CollisionDetector, rtc/ServoController, util/monitor] fixes some of compile warnings (`#1007 `) +* [hrpsys-base.pc] fix version in hrpsys-base.pc(`#1004 `_) +* [CMakeLists.txt] enables to use RelWithDebInfo (`#997 `_) +* [CMakeLists.txt] check version in CMakeLists.txt against package.xml (`#991 `_) +* [CMakeLists.txt] supports ubuntu16.04 (`#989 `_) + +* python + + * [python/rtm.py] checks if component is already activated/deactivated (`#1024 `_) + * [python/rtm.py] checks return values of activate_component() and deactivate_component() (`#1022 `_) + * [python/hrpsys_config.py] add method for setInterpolationMode (`#1012 `_) + * [python/hrpsys_config.py] save ReferenceForceUpdater output with DataLogger (`#1008 `_) + * [python/rtm.py] improves efficiency of readDataPort() by returning value of dataport.data_value property if available (`#1000 `_) + * [python/rtm.py] enables to specify interface_type (`#998 `_) + * [python/hrpsys_config.py] Fixed mistake of waitForRTCManagerAndRoboHardware's argument (`#988 `_) + * [pyrhon/hrpsys_config.py] Renamed waitForRTCManagerAndRoboHardware to waitForRTCManagerAndRobotHardware `#980 `_ (`#984 `_) + +* [doc] hrpsys_config.py comment update (`#1016 `_) + + * [doc for setJointAnglesSequence*] Wrong param type. Better description. + * [doc] Add in-code comment for the addition `#1012 `_. + +Unstable RTCs +============= + +* AutoBalancer + + * [rtc/AutoBalancer/AutoBalancer.cpp,rtc/Stabilizer/Stabilizer.cpp] Modify ref force output from ABC to ST (`#1035 `_) + + * add new stride limitation type when goPos and goVelocity (`#1031 `_) + + * [AutoBalancerService.idl, hrpsys_config.py, AutoBalancer.*, GaitGenerator.h, Stabilizer.*] add idl for leg_margin + * [samplerobot_auto_balancer.py] add test to check goPos when changing stride limitation type to circle + * [AutoBalancerService.idl, AutoBalancer.cpp, GaitGenerator.*] add new stride limitation type when goPos and goVelocity + * [GaitGenerator.*] fix region of stride limitation + + * add a function to limit stride (`#1029 `_) + + * [AutoBalancerService.idl, AutoBalancer.cpp, GaitGenerator.*] limit stride when use_stride_limitation is true + * [AutoBalancerService.idl, AutoBalancer.cpp, GaitGenerator.*] add function to limit stride + * [hrpsys_config.py, AutoBalancer.*, GaitGenerator.h, Stabilizer.*] get leg_margin from st + + * [AutoBalancer/PreviewController.h] get preview_control_gain f (`#1027 `_) + + * [rtc/AutoBalancer/AutoBalancer.{cpp,h}] print limb neame in the case of too large IK error. (`#1017 `_) + +* Stabilizer + + * Add new st distribution and add interpolator printing. (`#1013 `_) + + * [idl/StabilizerService.idl, rtc/Stabilizer/] Add new st_algorithm to check multi contact distribution. + * [rtc/Stabilizer/Stabilizer.cpp] Update checking of st_algorithm (TPCC or not). This commit should not change behavior. + * [rtc/SequencePlayer/interpolator.cpp] Add print message for MIN_INTERPOLATION_TIME in interpolator. + + * Update stabilizer sbp_cog_offset and add RobotHardware comment (`#987 `_) + + * [rtc/RobotHardware/robot.cpp] Add print message for removeForceSensorOffset and calibrateInertiaSensor + * [rtc/Stabilizer/Stabilizer.cpp] Fix orientation of sbp_cog_offset (reference world frame -> foot origin frame). + +* ReferenceForceUpdater + + * [rtc/ReferenceForceUpdater/,idl/ReferenceForceUpdaterService.idl] add frame parameter to set move_dir in world coordinates (`#1033 `_) + + * [rtc/ReferenceForceUpdater] Arrange Reference Force Updater(`#1010 `_) + + * [ReferenceForceUpdater/ReferenceForceUpdater.cpp] remove unused local variable, base_name_map. + * [ReferenceForceUpdater/ReferenceForceUpdater.cpp] remove unncessary lines: setting eet.sensor_name. + * [ReferenceForceUpdater/ReferenceForceUpdater.h] remove unused variable, m_data. + * [ReferenceForceUpdater/ReferenceForceUpdater.cpp] remove debug print in constructor and deconstructor because oth er rtc does not have debug print. + * [ReferenceForceUpdater/ReferenceForceUpdater.cpp] fix indent. + + * [rtc/ReferenceForceUpdater] set time for ReferenceForceUpdater output variable (`#1009 `_ ) + + * support both arm in ReferenceForceUpdater (`#1005 `_) + + * [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] fix indent + * [sample/SampleRobot/samplerobot_reference_force_updater.py] update rfu sample to check data port + * [idl/ReferenceForceUpdaterService.idl] remove arm parameter from ReferenceForceUpdaterParam and add arm arg to interfaces of rfu + [rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl{.h,.cpp}] remove arm parameter from ReferenceForceUpdaterParam + [rtc/ReferenceForceUpdater/ReferenceForceUpdater.h] add ReferenceForceUpdaterParam structure + [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] add Initialization for use_sh_base_pos_rpy + [rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp] enable to set both arms parameters independently in rfu + + * add sample and test for ReferenceForceUpdater(`#1003 `_) + + * [test] add test for ReferenceForceUpdater. + * [sample/SampleRobot] add sample for ReferenceForceUpdater. + +* TorqueFilter + + * [rtc/TorqueFilter/IIRFilter.h] Fix type of getCurrentValue (`#1032 `_) + +* Beeper + + * [rtc/Beeper] Update mutex lock and use buffer for communication between beep thread and real-time thread. (`#1030 `_) + +* SelfCollisionChecker + + * adds a tool, hrpsys-self-collision-checker (`#1026 `_) + +* OpenNIGrabber (`#1021 `_) + + * checks if OpenNI2 is installed + * makes error message from OpenNIGrabber more informative + * adds a configuration variable, mode + * improves error handling + * adds a new component, OpenNIGrabber + +* Contributors: Fumio Kanehiro, Isaac I.Y. Saito, Jun Inoue, Ryo Koyama, Kei Okada, Masaki Murooka, Noriaki Takasugi, Shunichi Nozawa, Yohei Kakiuchi, Yuta Kojio, Iori Yanokura + +315.9.0 (2016-04-19) +-------------------- + +Stable RTCs +============= + +* SequencePlayer + + * fix bug of setJointAnglesSequenceFull function in SequencePlayer (updated by snozawa) (`#908 `_) + + * [sample/SampleRobot/samplerobot_sequence_player.py] Add optionalData check and comment out clear joint angle check currently not working. + * [rtc/SequencePlayer/seqplay.cpp] Fix typo : RPY->ACC. + * rtc/SequencePlayer/SequencePlayer.cpp: fix bug of setJointAnglesSequenceFull function in SequencePlayer. + * rtc/SequencePlayer/SequencePlayerService_impl.cpp: fix the expected length of acc array in setJointAnglesSequenceFull. + * sample/SampleRobot/samplerobot_sequence_player.py: add test of setJointAnglesSequenceFull in samplerobot_sequence_player.py + + * sample/SampleRobot/samplerobot_sequence_player.py: remove joint group when finishing test. enable clear test of setJointAnglesSequenceFull. (`#914 `_) + +* DataLogger + + * Make more explicit the dependence of PointCloudViewer on VTK and on IO package of PCL (`#65 `_) + +* fix include directory for iob.h/idl/util (`#842 `_) + (io/iob.h -> hrpsys/io/iob.h, + xxx.hh -> hrpsys/idl/xxx.hh, + util/xxx.h -> hrpsys/util/xxx.h) + +* Update semaphore and EcexutionContext (`#970 `_) + + * [ec/hrpEC/hrpEC-common.cpp, hrpEC.h] Add ordinaly debug message for processing time if ENABLE_DEBUG_PRINT is true. Update print message for processing time. Move QNX ifdef for fprintf to header file. + * [rtc/RobotHardware/robot.[cpp,h] ,rtc/SequencePlayer/SequencePlayer.[cpp,h], rtc/StateHolder/StateHolder.[cpp,h]] Use semaphore.h instead of interprocess_semaphore because we do not use interprocess_semaphore specific functionality. On old OS, interprocess_semaphore cannot be used (boost version <= 1.35.0). This commit is related with the discussion : https://github.com/fkanehiro/hrpsys-base/issues/969 + +* rtm.py + + * rtm.py : fix wrong commit on #634 (isConnected() and False: ) (`#978 `_) + * test/test-hrpsysconf.py: add check do not connect again if already connected for https://github.com/fkanehiro/hrpsys-base/issues/979 + +* PDController + + * adds a function to interpolate reference angles (`#954 `_) + + * updates description of ref_dt + * adds a function to interpolate reference angles + +* lib/util/VectorConvert + * fixes a parse problem (`#954 `_) + +* Update docs (`#975 `_) + + * [idl/CollisionDetectorService.idl, RobotHardwareService.idl] Update documentation of idl + * [doc/Doxyfile.in, doc/package.h] Add beeper RTC documentation links. + * [README.md] Add documentation for directories, papers, and ros wiki + +* Fix include dir for QNX build (`#971 `_) + + * [rtc/ImpedanceController/ObjectTurnaroundDetector.h] Add including of Hrpsys.h to pass QNX build. + * [CMakeLists.txt] Specify include_directories as higher priority to pass QNX build. + +* [sample/SampleRobot/samplerobot_emergency_stopper.py,samplerobot_remove_force_offset.py] Use DataLogger instead of readDataPort for sample. (`#950 `_) + +* sample/SampleRobot/samplerobot_sequence_player.py: fix checkArrayBetween function (`#919 `_) + + * Add loadpatternst sample (`#907 `_) + + * [sample/SampleRobot/samplerobot_stabilizer.py] Add example for loadPattern + Stabilizer. + * [sample/SampleRobot/data, CMakeLists.txt] Add generated walking pattern file for SampleRobot. Add installation of data. + + * Update carry sample (`#909 `_) + + * [.travis.sh] Download and overwrite deb installed tests for downstream hrpsys-ros-bridge + * [sample/SampleRobot/samplerobot_carry_object.py] Define object turnaround detection time threshold and use hand fix mode during pushing manipulation. + * [sample/SampleRobot/README.md] Add conf file setting for el sample README. + + * Update st, abc, el, and travis.sh to pass travis tests (`#903 `_) + + * [sample/SampleRobot/samplerobot_soft_error_limiter.py] Check soft error limit checking after version '315.5.0' + * [.travis.sh] Add inputting of N for mongodb configuration during deb install reported in https://github.com/fkanehiro/hrpsys-base/pull/900#issuecomment-162392884 + * [sample/SampleRobot/samplerobot_soft_error_limiter.py] Use getActualState().command instead of rtm.readDataPort of el joint angle output to keep thread safety. + * [rtc/AutoBalancer/AutoBalancer.cpp] Revert AutoBalancer 7a8bc6781608d4251b6c268123d99781ea4d405b change which does not pass samplerobot_auto_balancer.py test. + * [sample/SampleRobot/samplerobot_auto_balancer.py] Use abs for Base RPY error checking and check base RPY error between reference and actual. + * [rtc/Stabilizer/Stabilizer.*] Update Stabilizer doc including paper names and equation numbers. + + * Update samplerobot python unittest (`#912 `_) + + * [sample/SampleRobot/samplerobot_sequence_player.py] Use StateHolder's getCommand to get seq results. + * [test/test-samplerobot-*.py] Use Unittest for samplerobot example testing to enable test results output and respawn of rostest. + * [.travis.sh] Print rosunit-*.xml if rostest fails + +* Fix pkg-config file, includedir should be the include directory, not the compiler flag. (`#947 `_) + +* [.travis.sh] Use --purge option for mongodb apt-get remove in order to remove configuration file. (reported in https://github.com/fkanehiro/hrpsys-base/pull/900#issuecomment-162392884) (`#906 `_) + + + +Unstable RTCs +============= + +* ImpedanceController + + * Fix bug of virtualforce (`#976 `_) + + * Modified Stabilizer to fix bug of virtual force + * Modified ImpedanceController for enabling VirtualForce + + * [rtc/ImpedanceController/ObjectTurnaroundDetector.h,sample/SampleRobot/samplerobot_impedance_controller.py] Fix to use round to convert double time parameter to size_t time count. (`#964 `_) + + * Add FFI for JointPathEx (`#938 `_) + + * [sample/euslisp/eus-joint-path-ex.l] Add FFI example using euslisp. Keeping hrpsys-base working without euslisp existence. + * [rtc/ImpedanceController/CMakeLists.txt, JointPathExC.cpp] Add JointPathEx example externed into C used for FFI. + + * [rtc/ImpedanceController/JointPathEx.cpp] Add debug print for nan from Inverse Kinematics calculations (`#925 `_) + +* ReferenceForceUpdater Add reference force updater (`#974 `_) + + * [doc/, rtc/ReferenceForceUpdater/ReferenceForceUpdater.txt] add document for ReferenceForceUpdater + * [python/hrpsys_config.py, launch/samplerobot.launch] enable to use rfu in robots + * [idl/, rtc/CMakeLists, rtc/ReferenceForceUpdater/] add new RTC named ReferenceForceUpdater(rfu) + +* SoftErrorLimitter + + * [rtc/SoftErrorLimiter/robot.cpp] Add print message for setServoErrorLimit (`#967 `_) + * [rtc/SoftErrorLimiter/SoftErrorLimiter.cpp, sample/SampleRobot/samplerobot_soft_error_limiter.py] Fix reference joint angle used to calculate error. Use joint angle which consider position limit and velocity limit. (`#966 `_) + +* Beeper + + * Add commenttoconnect and build beeper (`#964 `_) + + * [python/rtm.py] Add print message for dataflow_type, subscription_type, and so on. + * [rtc/CMakeLists.txt] Build Beeper RTC. + + * Add Beeper RTC (`#963 `_) + + * [python/hrpsys_config.py] Add Beeper to getUnstableRTC. Change order of el and tl to make tl higher priority for beeping. + * [rtc/SoftErrorLimiter/SoftErrorLimiter.*, rtc/CollisionDetector/CollisionDetector.*] Support both BeepClient to use BeeperRTC and start_beep for stable beeping RTCs because stable RTC does not support BeeperRTC. + * [rtc/ThermoLimiter/ThermoLimiter*, rtc/EmergencyStopper/EmergencyStopper.*] Use BeepClient to use BeeperRTC instead of start_beep for unstable beeping RTCs. + * [rtc/SoftErrorLimiter/beep.h] Add BeepClient class to use BeepRTC + * [rtc/Beeper] Add RTC to beep which takes input from several RTCs. + +* Kalman Filter + + * [EKFilter.h] fix typo : fussy -> fuzzy (`#958 `_) + + * Fussy tuned kalman filter (`#957 `_) + + * [KalmanFilter/EKFilter.h] use fuzzy logic to tune R matrix + * [samplerobot_kalman_filter.py] run test programs both with RPYKalmanFilter and with QuaternionExtendedKalmanFilter + * [samplerobot_kalman_filter.py] start auto balancer at the beginning to avoid slip + * [samplerobot_kalman_filter.py] compare kf_baseRpyCurrent with SampleRobot(Robot)0_WAIST not kf_rpy + + * Add quaternion kf test (`#956 `_) + + * [sample6dofrobot_kalman_filter.py.in] optimize label location + * [sample6dofrobot_kalman_filter.py.in] add quaternion estimator test + * [EKFilter.h, KalmanFilter.cpp] implement resetKalmanFilterState in EKFFilter + * [sample6dofrobot_kalman_filter.py.in] use actual rpy from simualtor + * [sample/Sample6dofRobot] rotate initial pose + + * Update quaternion ekf (`#955 `_) + + * [KalmanFilter/EKFilter.h] update coding styles for readability + * [KalmanFilter/EKFilter.h] refectering + * [KalmanFilter/EKFilter.h] use reference instead of returning value + * [KalmanFilter/EKFilter.h] clean up redundant codes + * [KalmanFilter/EKFilter.h] use rotation quaternion to rotate coordinate instead of rotation matrix + * [KalmanFilter/EKFilter.h] use hrpUtil to get Euler Angles from Rotation Matrix + * [KalmanFilter/EKFilter.h] use const reference parameters + * [KalmanFilter/EKFilter.h] do not pass a member variable to member functions + * [KalmanFilter/EKFilter.h] update calcF for readability + * [KalmanFilter/EKFilter.h] use const member functions + * [KalmanFilter/EKFilter.h] remove unused old comments + * [KalmanFilter/EKFilter.h] add a magic comment to use a 2 space indentation + * [KalmanFilter/EKFilter.h] use initializer list at EKFilter + * [KalmanFilter/EKFilter.h] Q should be gyro noise covariance in order to make it easy to tune parameters + * [KalmanFilter/EKFilter.h] normalize rotation quaternion as soon as possible + * [KalmanFilter/EKFilter.h] acceleration reference is to handle in KalmanFilter.cpp + * [KalmanFilter/EKFilter.h] fix bug : we should normalize only rotation quaternion + +* TorqueController + + * Add getter method to torque controller (`#933 `_) + + * [TorqueController] Fix transition time expression bag + * [TorqueController] Rename paramter argument name in torque controller to corersponding rtm-ros-robot-interface: t_param -> i_param + * [TorqueController] Add get parameter methods for torque controller + + * Fix torque controller pass qref mode (`#926 `_) + + * [TorqueController] Fix merge miss in timestamp + * [TorqueController] Supress dq from torque controller by min/max_dq + * [TorqueController] Pass qRefIn without checking range of motion when motor torque contorller is disabled + * [MotorTorqueController] tauMax should be not zero when tau is zero + * [TorqueController] Check size of qRef to prevent accessing qRefIn when qRefIn size is not same as joint_num + +* AutoBalancer + + * [rtc/AutoBalancer/GaitGenerator.h] Add boundary conditions of velocity and acceleration to GaitGenerator (`#981 `_) + * [rtc/AutoBalancer/GaitGenerator.h] Fix zmp weight interpolation and use setGoal instead of go. (`#973 `_) + + * Get foosteps (`#939 `_) + + * [AutoBalancer.cpp, GaitGenerator.cpp, GaitGenerator.h] use const member function in getGoPosFootstepsSequence + * [GaitGenerator.cpp, GaitGenerator.h] pass vel_param for argument in go_pos_param_2_footstep_nodes_list + * [AutoBalancerService.idl, AutoBalancer.cpp, AutoBalancer.h, AutoBalancerService_impl.cpp, AutoBalancerService_impl.h] add getFootstepsSequence function + * [GaitGenerator.cpp, GaitGenerator.h] overload go_pos_param_2_footstep_nodes_list to get new_footstep_nodes_list + * [AutoBalancer.cpp, AutoBalancer.h] move initial_support_legs calculation method from inside goPos to a new method + + * [rtc/AutoBalancer/AutoBalancer.cpp] Initialize gait_type as BIPED. (`#937 `_) + + * Update JointPathEx IK (`#942 `_) + + * [idl/AutoBalancerService.idl,idl/StabilizerService.idl,rtc/AutoBalancer/AutoBalancer.*,rtc/Stabilizer/Stabilizer.cpp] Enable to set IK weight vector for STtabilizer and Autobalancer like ImpedanceController. + * [rtc/ImpedanceController/ImpedanceController.cpp,rtc/ImpedanceController/JointPathEx.*,rtc/Stabilizer/Stabilizer.cpp] Move end-effector version inverse kinematics to JointPathEx and use it in IC and ST. + * [rtc/ImpedanceController/ImpedanceController.cpp,JointPathEx.*,rtc/Stabilizer/Stabilizer.cpp,rtc/AutoBalancer/AutoBalancer.cpp] Add calcInverseKinematics2Loop function to take target pos and Rot and use it in ic, abc, and st. Currently omegaFromRot is under checking and tempolarily use old matrix_log function, so program behaviour does not change. + * [idl/AutoBalancerService.idl,rtc/AutoBalancer/AutoBalancer.*] Remove deprecated footstep information lleg_coords and rleg_coords. Remove unused current\_* parameter from ABCIKparam. + + * fix bug when overwriting footstep (`#940 `_) + + * [rtc/AutoBalancer/GaitGenerator.cpp, rtc/AutoBalancer/PreviewController.h] fix bug when overwriting footstep + * [sample/SampleRobot/samplerobot_auto_balancer.py] Add checking for discontinuity of COG trajectory during footstep overwriting by checking COG too large acc. + + * Update gaitgenerator and fix bugs (`#918 `_) + + * [rtc/AutoBalancer/GaitGenerator.cpp,h] Add get_overwrite_check_timing + * [.travis.sh] Print if rosunit_xml_result_files exists + * [rtc/AutoBalancer/GaitGenerator.cpp] Enable emergencyStop for walking anytime. Previously, emergency flag is checked at half of step time. + * [rtc/AutoBalancer/GaitGenerator.cpp] Set toe heel time count based on each footstep step count + + * Update gopos (`#877 `_) + + * [sample/SampleRobot/samplerobot_impedance_controller.py] Check hrpsys_version for samplerobot impedance test + * [rtc/ImpedanceController/JointPathEx.*, AutoBalancer, ImpedanceController, SequencePlayer, Stabilizer] Reduce limit over print message frequence in JointPathEx and add more information for it. + * [sample/SampleRobot/samplerobot_impedance_controller.py, test/test-samplerobot-impedance.py] Test samplerobot_impedance_controller python example + * [rtc/*] Update print message from RTCs like [el] + * [rtc/AutoBalancer/GaitGenerator.*] Update appending of footstep function. Define both const and non-const member function. + * [rtc/AutoBalancer/GaitGenerator.*, AutoBalancer.cpp] Enable to overwrite goPos target goal. + * [sample/SampleRobot/samplerobot_auto_balancer.py] Add check test for goPos final dst_foot_midcoords and add example for goPos overwrite. + * [rtc/AutoBalancer/GaitGenerator.h] Set is_initialize for gopos true by default to pass tests with default argument. + * [rtc/AutoBalancer/GaitGenerator.*] Use const member function for getter and printing functions. + + + * Overwrite current footstep (`#916 `_) + + * [rtc/AutoBalancer/GaitGenerator.cpp] Fix for future velocity footsteps. Integrate future steps. + * [sample/SampleRobot/samplerobot_auto_balancer.py] Add example for current footstep overwrite + * [rtc/AutoBalancer/GaitGenerator.cpp,rtc/AutoBalancer/PreviewController.h] Enable to set overwritable_footstep_index_offset = 0. + * [idl/AutoBalancerService.idl, rtc/AutoBalancer/AutoBalancer.cpp,GaitGenerator.h] Enable to set overwritable_footstep_index_offset. + * [rtc/AutoBalancer/GaitGenerator.cpp] Fix order of overwrite zmp processing and add comments. This should not change behaviour. + * [rtc/AutoBalancer/GaitGenerator.cpp,h] Enable to set future_step_num and use get_overwritable_index. + +* Stabilizer + + * Add refforce weight to eefmqp (`#977 `_) + + * [rtc/Stabilizer/Stabilizer.cpp,ZMPDistributor] Add ref force weight to eefmqp + * [rtc/AutoBalancer/AutoBalancer.cpp] Modify ref force output + + * [rtc/Stabilizer/ZMPDistributor.h] do not distribute ForceMoment to swing foot (`#972 `_) + + * Add fall direction (`#948 `_) + + * merge origin/master by hand + * [AutoBalancer.cpp] stop walking if emergency signal is set + * [Stabilizer.cpp] check single support phase only in wailking for recovery + * [StabilizerService.idl, Stabilizer.cpp, Stabilizer.h] add tilt_margin parameter for single support phase and double support phase + * [StabilizerService.idl, Stabilizer.cpp] add TILT emergency mode + * [Stabilizer.cpp, Stabilizer.h] add fall direction caulculator + + + * Fix abc st segfo (`#951 `_) + + * [rtc/AutoBalancer/AutoBalancer.cpp] Fix initialization of target_p0 and target_r0 + * [rtc/Stabilizer/ZMPDistributor.h] Check size of ee params to avoid segfo. + + * [Stabilizer.cpp, Stabilizer.h] fix swing leg modification rule (`#949 `_) + + * [StabilizerService.idl, Stabilizer.cpp, Stabilizer.h] add eefm_swing_pos_time_const/eefm_swing_rot_time_const parameter (`#949 `_) + + * Add argument check st abc (`#945 `_) + + * [sample/SampleRobot/samplerobot\_*.py] Use DataLogger log for check robot's state for testing + * [test/test-samplerobot.test] Set order for samplerobot test execution. For example, DataLogger, SequencePlayer, ... + * [rtc/AutoBalancer/AutoBalancer.cpp,rtc/Stabilizer/Stabilizer.cpp] Fix location of set Ik parameter and add comments and message + * [rtc/AutoBalancer/AutoBalancer.cpp,rtc/Stabilizer/Stabilizer.cpp] Add argument length check for IK parameter for AutoBalancer and Stabilizer + + * [idl/AutoBalancerService.idl,idl/StabilizerService.idl,rtc/AutoBalancer/AutoBalancer.cpp,rtc/Stabilizer/Stabilizer.cpp] Use IKLimbParameters instead of each sequence paraemters for IK of AutoBalancer and Stabilizer. (`#944 `_) + + * [idl/AutoBalancerService.idl,idl/StabilizerService.idl,rtc/AutoBalancer/AutoBalancer.*,rtc/Stabilizer/Stabilizer.*] Add IK parameter interface for AutoBalancer and Stabilizer. (`#943 `_) + + * Add moment limit and test for turnwalk (`#936 `_) + + * [sample/SampleRobot/samplerobot_stabilizer.py] Add test for turn walk abount 180[deg] yaw rotation. + * [idl/StabilizerService.idl, rtc/Stabilizer/Stabilizer.*] Add limitation for end-effector frame local reference moment to avoid hardware break. + + + [Stabilizer.cpp] match f_diff frame to ref_f_diff one (`#935 `_) + + * Fix fixed coords again (`#917 `_) + + * [AutoBalancer.cpp] resize leg_pos to end-effector size + * [AutoBalancer.cpp] move leg_pos initialization position + * [Stabilizer.cpp] match f_diff frame to ref_f_diff one + * [sample4legrobot_auto_balancer.py] add goVelocity in trot sample + * [AutoBalancer.cpp, GaitGenerator.cpp, GaitGenerator.h] support multileg in go velocity + * [GaitGenerator.cpp, GaitGenerator.h] support multi legs for overwirte + * [sample4legrobot_auto_balancer.py] add goPos in trot and in pace samples + * [sample4legrobot_auto_balancer.py] move all end-pos +50mm in z axis to get manipulability + * [AutoBalancerService.idl, AutoBalancer.cpp, AutoBalancer.h] add gait_type to AutoBalancer param to realize multiple gait in goPos + * [sample4legrobot_auto_balancer.py] add crawl mode test + * [AutoBalancer.cpp, GaitGenerator.h] fix fixed coordinates in multiple legs : only use legs / re-revert and update https://github.com/fkanehiro/hrpsys-base/commit/ad4eb10d05f98aca9f243bb72a81ffba4b51dd77 + + * Modify swing leg end coords (`#934 `_) + + * [Stabilizer.cpp] add a new modification law as a comment + * [Stabilizer.cpp] modify swing leg coods only in actual and reference swing time + * [Stabilizer.cpp] calulate difference rpy in new_swg_R coordinates at swing leg modification + * [Stabilizer.cpp] print d_rpy_swing / d_pos_swing for DEBUG + * [Stabilizer.cpp] support multiple legs for swing leg modification + * [Stabilizer.cpp] separte rotation scope and position one for readability + * [Stabilizer.cpp, Stabilizer.h] rename delta_pos / delta_rpy to d_rpy_swing / d_pos_swing and keep these variables as member variables for extensibility + * [Stabilizer.cpp] use eefm_swing_rot_spring_gain / eefm_swing_rot_spring_gain param + * [StabilizerService.idl, Stabilizer.cpp, Stabilizer.h] add eefm_swing_rot_spring_gain / eefm_swing_rot_spring_gain as st param + * [Stabilizer.cpp, Stabilizer.h] modify swing leg end-coords to follow target one in world coordinates + + * Return total force or moment from getObjectForcesMoments and consider moment_center as foot mid frame. (`#932 `_) + + * [rtc/ImpedanceController/ImpedanceController.cpp] Check for legged robot. + * [rtc/ImpedanceController/ObjectTurnaroundDetector.h] Reset current filtered param when detect mode switched. + * [idl/ImpedanceControllerService.idl, rtc/ImpedanceController/*] Return total force or moment from getObjectForcesMoments and consider moment_center as foot mid frame. + + * Enable total moment detection by object turnaround detection. (`#930 `_) + + * [idl/ImpedanceControllerService.idl,rtc/ImpedanceController/ImpedanceController.cpp,rtc/ImpedanceController/ImpedanceController.h,rtc/ImpedanceController/ObjectTurnaroundDetector.h] Enable total moment detection by object turnaround detection. + + * [Stabilizer.cpp] enable to change compensation limit : omission of https://github.com/fkanehiro/hrpsys-base/pull/852 (`#929 `_) + + * [Stabilizer.cpp] fix com height of LIPM in Capture Point calculation (`#924 `_) + + * [hrpsys_config.py] start stabilizer after auto-balancer in startDefaultUnstableControllers (`#928 `_) + + * [Stabilizer.cpp] fix typo of https://github.com/fkanehiro/hrpsys-base/pull/895 (`#922 `_) + + * Update graspless manip mode (`#921 `_) + + * [sample/SampleRobot/samplerobot_auto_balancer.py] Add example for dual-arm graspless manip walking. + * [rtc/AutoBalancer/AutoBalancer.cpp] Support dual-arm graspless mode while walking. + + * Add rostest for stabilizer. (`#910 `_) + + * [sample/SampleRobot/samplerobot_stabilizer.py] Check existence of sample1_bush.wrl because openhrp3 <= 3.1.8 does not have it. + * [test/test-samplerobot-st.test, .travis.yml] Add test for samplerobot st with torque + pdcontrol + bush. Add travis job for testing st test. + + +* RangeDataViewer + + * rtc/RangeDataViewer/RangeDataViewer.cpp: suppresses debug messages and ignores inf + +* get all q log (`#915 `_) + + * [CollisionDetector.cpp] set timestamp for out port + * [TorqueController.cpp] use upstream timestamp instead of current timestamp + + +* Contributors: Benjamin Chrétien, Eisoku Kuroiwa, Fumio Kanehiro, Iori Kumagai, Kei Okada, Kohei Kimura, Masaki Murooka, Mehdi Benallegue, Ryo Koyama, Shunichi Nozawa, Takasugi Noriaki, Yohei Kakiuchi, Yuta Kojio, Iori Yanokura + 315.8.0 (2015-11-29) -------------------- diff --git a/CMakeLists.txt b/CMakeLists.txt index ccafb092a6e..d00e9a7d0a6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -17,6 +17,21 @@ option(ENABLE_INSTALL_RPATH "Enable RPATH setting for installed binary files" OF set(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib") set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) +option(USE_SANITIZER "Enable address sanitizer" OFF) +message(STATUS "USE_SANITIZER: ${USE_SANITIZER}") +if(USE_SANITIZER) + message(STATUS "Enable Sanitizer") + set(SANITIZE_FLAGS "-fsanitize=address") + set(CMAKE_CXX_FLAGS "${SANITIZE_FLAGS}") + set(CMAKE_C_FLAGS "${SANITIZE_FLAGS}") + set(CMAKE_CXX_FLAGS_DEBUG "${SANITIZE_FLAGS}") + set(CMAKE_C_FLAGS_DEBUG "${SANITIZE_FLAGS}") + set(CMAKE_CXX_FLAGS_RELEASE "${SANITIZE_FLAGS}") + set(CMAKE_C_FLAGS_RELEASE "${SANITIZE_FLAGS}") + set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${SANITIZE_FLAGS}") + set(CMAKE_C_FLAGS_RELWITHDEBINFO "${SANITIZE_FLAGS}") +endif() + # commands if(UNIX) set(RMDIR rm -fr) @@ -33,9 +48,33 @@ elseif(WIN32) set(SH_SFX "bat") endif() +# Specify include directory. +# After https://github.com/fkanehiro/hrpsys-base/pull/842, hrpsys-base local include files are used as "hrpsys/xx/yy.h". +# For this reason, we make ${CMAKE_BINARY_DIR}/hrpsys directory and generate symbolic link for related include files under it. +# To deal with include files in ${CMAKE_BINARY_DIR}/hrpsys as hrpsys-base local include files, we set cmake's include_directories before other include_directories setting to set higher priority for ${CMAKE_BINARY_DIR}/hrpsys. +execute_process( + COMMAND mkdir -p ${CMAKE_BINARY_DIR}/hrpsys +) +if (NOT EXISTS ${CMAKE_BINARY_DIR}/hrpsys/io) + execute_process( + COMMAND ln -s ${PROJECT_SOURCE_DIR}/lib/io ${CMAKE_BINARY_DIR}/hrpsys/io + ) +endif() +if (NOT EXISTS ${CMAKE_BINARY_DIR}/hrpsys/util) + execute_process( + COMMAND ln -s ${PROJECT_SOURCE_DIR}/lib/util ${CMAKE_BINARY_DIR}/hrpsys/util + ) +endif() +if (NOT EXISTS ${CMAKE_BINARY_DIR}/hrpsys/idl) + execute_process( + COMMAND ln -s ${CMAKE_BINARY_DIR}/idl ${CMAKE_BINARY_DIR}/hrpsys/idl + ) +endif() +include_directories(${CMAKE_BINARY_DIR}) + # OpenRTM-aist(>= 1.0.0) find_package(OpenRTM REQUIRED) -if(EXISTS "${OPENRTM_DIR}/include/openrtm-1.1/rtm/RTObjectStateMachine.h") +if(EXISTS "${OPENRTM_DIR}/include/openrtm-1.2/rtm/RTObjectStateMachine.h") add_definitions(-DOPENRTM_VERSION_TRUNK) endif() @@ -44,11 +83,20 @@ find_package(OpenHRP REQUIRED) include_directories(${OPENHRP_INCLUDE_DIRS}) link_directories(${OPENHRP_LIBRARY_DIRS}) if(UNIX) + # remove -O2 ... from ${OPENHRP_DEFINITIONS} + message(STATUS "OPENHRP_DEFINITIONS: ${OPENHRP_DEFINITIONS}") + message(STATUS "REMOVE -Ox option from OPENHRP_DEFINITIONS, use CMAKE_BUILD_TYPE") + string(REGEX REPLACE "[ ]+-O[0-9][ ]+" " " OPENHRP_DEFINITIONS "${OPENHRP_DEFINITIONS}") + message(STATUS "OPENHRP_DEFINITIONS: ${OPENHRP_DEFINITIONS}") add_definitions(${OPENHRP_DEFINITIONS}) endif() - -include_directories(${PROJECT_SOURCE_DIR}/lib) -include_directories(${CMAKE_BINARY_DIR}/idl) +add_definitions(-DOPENHRP_PACKAGE_VERSION=\"\\"${OPENHRP_VERSION}\\"\") +message(STATUS "OpenHRP3 Version : ${OPENHRP_VERSION}") +if ("${OPENHRP_VERSION}" VERSION_GREATER "3.1.9") + message(STATUS "enable OpenHRP3 3.2") + add_definitions(-DOPENHRP_PACKAGE_VERSION_320) +endif() +add_definitions(-DOPENHRP_PACKAGE_VERSION_320) ### cheat ## if(ENABLE_INSTALL_RPATH) set(CMAKE_INSTALL_RPATH "$ORIGIN/../lib:$ORIGIN/../../../lib:${OPENHRP_LIBRARY_DIRS}") @@ -70,12 +118,17 @@ if(COMPILE_JAVA_STUFF) set(javac_flags -target ${Java_VERSION_MAJOR}.${Java_VERSION_MINOR} -d . -sourcepath src) endif() -if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL None OR CMAKE_BUILD_TYPE STREQUAL RelWithDebInfo) +if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL None) set( CMAKE_BUILD_TYPE Release CACHE STRING "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." FORCE) endif() +message(STATUS "Compile with ${CMAKE_BUILD_TYPE}") +message(STATUS " -- CMAKE_C_FLAGS / CMAKE_CXX_FLAGS ${CMAKE_C_FLAGS} / ${CMAKE_C_FLAGS}") +message(STATUS " -- CMAKE_C_FLAGS_DEBUG / CMAKE_CXX_FLAGS_DEBUG ${CMAKE_C_FLAGS_DEBUG} / ${CMAKE_C_FLAGS_DEBUG}") +message(STATUS " -- CMAKE_C_FLAGS_RELEASE / CMAKE_CXX_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE} / ${CMAKE_C_FLAGS_RELEASE}") +message(STATUS " -- CMAKE_C_FLAGS_RELWITHDEBINFO / CMAKE_CXX_FLAGS_RELWITHDEBINFO ${CMAKE_C_FLAGS_RELWITHDEBINFO} / ${CMAKE_C_FLAGS_RELWITHDEBINFO}") set(LIBRARY_OUTPUT_PATH ${PROJECT_BINARY_DIR}/lib) set(EXECUTABLE_OUTPUT_PATH ${PROJECT_BINARY_DIR}/bin) @@ -84,7 +137,7 @@ set(CPACK_PACKAGE_NAME hrpsys-base) set(CPACK_PACKAGE_VENDOR "AIST") set(CPACK_PACKAGE_CONTACT "Fumio Kanehiro ") set(CPACK_PACKAGE_VERSION_MAJOR 315) -set(CPACK_PACKAGE_VERSION_MINOR 7) +set(CPACK_PACKAGE_VERSION_MINOR 15) set(CPACK_PACKAGE_VERSION_PATCH 0) execute_process( COMMAND date +%Y%m%d%H%M @@ -119,7 +172,8 @@ endif( NOT GLUT_FOUND ) include(FindPkgConfig) pkg_check_modules(OpenCV opencv) if (NOT OpenCV_FOUND) - pkg_check_modules(OpenCV opencv-2.3.1) + # pkg_check_modules(OpenCV opencv-2.3.1) + pkg_check_modules(OpenCV opencv-3.3.1-dev) if (NOT OpenCV_FOUND) message(WARNING "opencv not found") endif() @@ -128,8 +182,17 @@ pkg_check_modules(GLEW glew) if (NOT GLEW_FOUND) message(WARNING "Package GLEW is not found") endif() +find_package(PCL) if(OpenCV_FOUND AND OPENGL_FOUND AND GLEW_FOUND AND GLUT_FOUND AND SDL_FOUND) - set(USE_HRPSYSUTIL true) + if(APPLE) + if(PCL_FOUND) # qhull is included in pcl + set(USE_HRPSYSUTIL true) + else() + set(USE_HRPSYSUTIL false) + endif() + else() + set(USE_HRPSYSUTIL true) + endif() else() set(USE_HRPSYSUTIL false) endif() @@ -160,10 +223,7 @@ if(NO_DIGITAL_INPUT) message(STATUS "Disablng readDigitalInput and lengthDigitalInput") endif() -option(ROBOT_IOB_VERSION "Supported robot IOB version (lib/io/iob.h)") -if("${ROBOT_IOB_VERSION}" STREQUAL "OFF") - set(ROBOT_IOB_VERSION 2) -endif() +set(ROBOT_IOB_VERSION 4 CACHE STRING "Supported robot IOB version (lib/io/iob.h)") # can be overwritten by extra compile options or ccmake add_definitions(-DROBOT_IOB_VERSION=${ROBOT_IOB_VERSION}) message(STATUS "compile iob with -DROBOT_IOB_VERSION=${ROBOT_IOB_VERSION}") @@ -175,7 +235,7 @@ add_subdirectory(idl) add_subdirectory(lib) add_subdirectory(ec) # option for 3rdparty -option(USE_QPOASES "Build qpOASES" OFF) +option(USE_QPOASES "Build qpOASES" ON) add_subdirectory(3rdparty) add_subdirectory(rtc) option(ENABLE_DOXYGEN "Use Doxygen" ON) @@ -191,6 +251,13 @@ add_subdirectory(test) #if catkin environment string(REGEX MATCH "catkin" need_catkin "$ENV{_}") if(need_catkin OR "${CATKIN_BUILD_BINARY_PACKAGE}") + find_package(catkin) + if(catkin_FOUND) + catkin_package_xml() + if(NOT hrpsys_VERSION VERSION_EQUAL CPACK_PACKAGE_VERSION) + message( FATAL_ERROR "hrpsys_version (from package.xml): ${hrpsys_VERSION} is not equal CPACK_PACKAGE_VERSION: ${CPACK_PACKAGE_VERSION}" ) + endif() + endif() install(CODE "file(WRITE \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/share/doc/hrpsys/rospack_nosubdirs)") install(FILES package.xml DESTINATION share/hrpsys/) install(CODE " @@ -234,7 +301,9 @@ if(EXISTS \${_catkin_marker_file}) # append to existing list of sourcespaces if it's not in the list list(FIND _existing_sourcespaces \"${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}\" _existing_sourcespace_index) if(_existing_sourcespace_index EQUAL -1) - file(APPEND \${_catkin_marker_file} \";${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}\") + set(_catkin_marker_file_content \" ${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}\") # head space is necessary because it is converted to semicolon. + separate_arguments(_catkin_marker_file_content) + file(APPEND \${_catkin_marker_file} \"${_catkin_marker_file_content}\") endif() endif() else() diff --git a/README.md b/README.md index 5013a6559ef..f3fad8b72c7 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,51 @@ [![Build Status](https://travis-ci.org/fkanehiro/hrpsys-base.svg?branch=master)](https://travis-ci.org/fkanehiro/hrpsys-base) +# Install + For Install, See [INSTALL](https://raw.githubusercontent.com/fkanehiro/hrpsys-base/master/INSTALL) +# API + For API, See [API Doc](http://fkanehiro.github.io/hrpsys-base/) +# Samples + For Samples, See [Samples](https://github.com/fkanehiro/hrpsys-base/tree/master/sample) +For related informations: + +ROS integration with hrpsys-base [ROS wiki hrpsys page](http://wiki.ros.org/rtmros_common/Tutorials/GettingStart) + +Example case of system configurations is written in the following paper Fig.3: +Shunichi Nozawa, Eisoku Kuroiwa, Kunio Kojima, Ryohei Ueda, Masaki Murooka, Shintaro Noda, +Iori Kumagai, Yu Ohara, Yohei Kakiuchi, Kei Okada, Masayuki Inaba, +"Multi-layered real-time controllers for humanoid's manipulation and locomotion tasks with emergency stop", +2015 IEEE-RAS 15th International Conference on Humanoid Robots (Humanoids), pp.381-388, 2015 + +# Directories +Main directories are here (for more information, please see API Doc): +## 3rdparty +3rd party software installation cmake and settings. +## ec +For execution context implementations for real-time linux used for hrpsys-base software. +## python/jython +python/jython files. +## lib +Library programs which can be used from outside of hrpsys-base. +For example, IO libraries. +## sample +Sample programs. See Samples. +## util +Utilities. +## doc +Doxygen files. +Substantial documentation files are in each directories such as rtc/XX/XX.txt. +## idl +IDL files used for each RTC. +## launch +ROS related programs. +## test +Test codes including python samples and rostest. +## rtc +RT-Component implementations. +Basically, one directory corresponds to one RT-Component. diff --git a/cmake_modules/FindOpenRTM.cmake b/cmake_modules/FindOpenRTM.cmake index 8188b347de0..04f04828b3e 100644 --- a/cmake_modules/FindOpenRTM.cmake +++ b/cmake_modules/FindOpenRTM.cmake @@ -42,6 +42,10 @@ if(UNIX) set(OPENRTM_IDL_DIR ${OPENRTM_DIR}/include/openrtm-1.1/rtm/idl) list(APPEND OPENRTM_INCLUDE_DIRS "${OPENRTM_DIR}/include/openrtm-1.1") list(APPEND OPENRTM_INCLUDE_DIRS "${OPENRTM_DIR}/include/openrtm-1.1/rtm/idl") + elseif(NOT (OPENRTM_VERSION VERSION_LESS "1.2.0")) + set(OPENRTM_IDL_DIR ${OPENRTM_DIR}/include/openrtm-1.2/rtm/idl) + list(APPEND OPENRTM_INCLUDE_DIRS "${OPENRTM_DIR}/include/openrtm-1.2") + list(APPEND OPENRTM_INCLUDE_DIRS "${OPENRTM_DIR}/include/openrtm-1.2/rtm/idl") else() set(OPENRTM_IDL_DIR ${OPENRTM_DIR}/include/rtm/idl) list(APPEND OPENRTM_INCLUDE_DIRS "${OPENRTM_DIR}/include") diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt index 7e10c0a7309..989740266d1 100644 --- a/doc/CMakeLists.txt +++ b/doc/CMakeLists.txt @@ -1,7 +1,9 @@ set(input_files package.h utilities.h ../python/hrpsys_config.py ../python/rtm.py + ../rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.txt ../rtc/CameraImageViewer/CameraImageViewer.txt + ../rtc/CameraImageSaver/CameraImageSaver.txt ../rtc/DataLogger/DataLogger.txt ../rtc/ForwardKinematics/ForwardKinematics.txt ../rtc/HGcontroller/HGcontroller.txt @@ -12,8 +14,11 @@ set(input_files package.h utilities.h ../rtc/JpegDecoder/JpegDecoder.txt ../rtc/NullComponent/NullComponent.txt ../rtc/EmergencyStopper/EmergencyStopper.txt + ../rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.txt ../rtc/OccupancyGridMap3D/OccupancyGridMap3D.txt ../rtc/OGMap3DViewer/OGMap3DViewer.txt + ../rtc/OpenNIGrabber/OpenNIGrabber.txt + ../rtc/PointCloudViewer/PointCloudViewer.txt ../rtc/Range2PointCloud/Range2PointCloud.txt ../rtc/RobotHardware/RobotHardware.txt ../rtc/SequencePlayer/SequencePlayer.txt @@ -29,6 +34,7 @@ set(input_files package.h utilities.h ../rtc/AutoBalancer/AutoBalancer.txt ../rtc/KalmanFilter/KalmanFilter.txt ../rtc/ImpedanceController/ImpedanceController.txt + ../rtc/ReferenceForceUpdater/ReferenceForceUpdater.txt ../rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt ../rtc/CollisionDetector/CollisionDetector.txt ../rtc/PDcontroller/PDcontroller.txt @@ -40,6 +46,7 @@ set(input_files package.h utilities.h ../idl/GraspControllerService.idl ../idl/ImpedanceControllerService.idl ../idl/OGMap3DService.idl + ../idl/ObjectContactTurnaroundDetectorService.idl ../idl/RobotHardwareService.idl ../idl/SequencePlayerService.idl ../idl/StateHolderService.idl @@ -49,6 +56,7 @@ set(input_files package.h utilities.h ../idl/StabilizerService.idl ../idl/KalmanFilterService.idl ../idl/RemoveForceSensorLinkOffsetService.idl + ../idl/ReferenceForceUpdaterService.idl ) configure_file(Doxyfile.in ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile) set(output_dir ${CMAKE_CURRENT_BINARY_DIR}/html) diff --git a/doc/Doxyfile.in b/doc/Doxyfile.in index a004cf6ba72..7dea2fbbe4b 100644 --- a/doc/Doxyfile.in +++ b/doc/Doxyfile.in @@ -469,9 +469,12 @@ INPUT = @CMAKE_CURRENT_SOURCE_DIR@/package.h \ @CMAKE_CURRENT_SOURCE_DIR@/../python/hrpsys_config.py \ @CMAKE_CURRENT_SOURCE_DIR@/../python/rtm.py \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/AccelerationChecker \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/ApproximateVoxelGridFilter \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/AutoBalancer \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/AverageFilter \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/Beeper \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/CameraImageViewer \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/CameraImageSaver \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/CaptureController \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/CollisionDetector \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/DataLogger \ @@ -491,15 +494,19 @@ INPUT = @CMAKE_CURRENT_SOURCE_DIR@/package.h \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/MLSFilter \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/NullComponent \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/EmergencyStopper \ - @CMAKE_CURRENT_SOURCE_DIR@/../rtc/OGMap3DViewer \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/ObjectContactTurnaroundDetector \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/OccupancyGridMap3D \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/OGMap3DViewer \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/OpenNIGrabber \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/PCDLoader \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/PDcontroller \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/PlaneRemover \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/PointCloudViewer \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/RGB2Gray \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/Range2PointCloud \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/RangeDataViewer \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/RangeNoiseMixer \ + @CMAKE_CURRENT_SOURCE_DIR@/../rtc/ReferenceForceUpdater \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/RemoveForceSensorLinkOffset \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/ResizeImage \ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/RobotHardware \ diff --git a/doc/package.h b/doc/package.h index 14a7e64135b..f0c06a158e6 100644 --- a/doc/package.h +++ b/doc/package.h @@ -46,10 +46,14 @@ To use python scripts to create RT components, connect ports and get/set propert
  • \ref AccelerationChecker
  • \ref AutoBalancer
    • +
  • \ref ApproximateVoxelGridFilter
  • \ref AverageFilter
    • +
  • \ref Beeper
  • \ref CameraImageViewer
    • +
  • \ref CameraImageSaver
  • \ref CaptureController
  • \ref CollisionDetector
    • +
  • \ref ColorExtractor
  • \ref DataLogger
  • \ref ExtractCameraImage
  • \ref ForwardKinematics
    • @@ -67,15 +71,19 @@ To use python scripts to create RT components, connect ports and get/set propert
  • \ref MLSFilter
  • \ref NullComponent
  • \ref EmergencyStopper
    • -
  • \ref OGMap3DViewer
    • +
  • \ref ObjectContactTurnaroundDetector
  • \ref OccupancyGridMap3D
    • +
  • \ref OGMap3DViewer
    • +
  • \ref OpenNIGrabber
  • \ref PCDLoader
  • \ref PDcontroller
  • \ref PlaneRemover
    • +
  • \ref PointCloudViewer
  • \ref RGB2Gray
  • \ref Range2PointCloud
  • \ref RangeDataViewer
  • \ref RangeNoiseMixer
    • +
  • \ref ReferenceForceUpdater
  • \ref RemoveForceSensorLinkOffset
  • \ref ResizeImage
  • \ref RobotHardware
    • diff --git a/ec/hrpEC/CMakeLists.txt b/ec/hrpEC/CMakeLists.txt index 28b5969df77..4e0b7ad3c46 100644 --- a/ec/hrpEC/CMakeLists.txt +++ b/ec/hrpEC/CMakeLists.txt @@ -17,8 +17,8 @@ endif() set_target_properties(hrpEC PROPERTIES PREFIX "") install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/ec/hrpEC/hrpEC-art.cpp b/ec/hrpEC/hrpEC-art.cpp index 45c2e78408f..cc465fc4451 100644 --- a/ec/hrpEC/hrpEC-art.cpp +++ b/ec/hrpEC/hrpEC-art.cpp @@ -2,14 +2,15 @@ #include #include "hrpEC.h" #include -#include "io/iob.h" +#include "hrpsys/io/iob.h" #include namespace RTC { hrpExecutionContext::hrpExecutionContext() : PeriodicExecutionContext(), - m_priority(ART_PRIO_MAX-1) + m_priority(ART_PRIO_MAX-1), + m_thread_pending (false) { resetProfile(); rtclog.setName("hrpEC"); diff --git a/ec/hrpEC/hrpEC-common.cpp b/ec/hrpEC/hrpEC-common.cpp index 0eb0cd10e40..db16eaccc63 100644 --- a/ec/hrpEC/hrpEC-common.cpp +++ b/ec/hrpEC/hrpEC-common.cpp @@ -1,19 +1,27 @@ #include "hrpEC.h" -#include "io/iob.h" +#include "hrpsys/io/iob.h" #ifdef OPENRTM_VERSION_TRUNK #include #endif -#ifdef __QNX__ -using std::fprintf; -#endif +//#define ENABLE_DEBUG_PRINT (true) +#define ENABLE_DEBUG_PRINT (false) namespace RTC { hrpExecutionContext::~hrpExecutionContext() { + wait(); + if (m_thread_pending) + abort (); } int hrpExecutionContext::svc(void) + { + int ret = svc_wrapped (); + m_thread_pending = false; + return ret; + } + int hrpExecutionContext::svc_wrapped(void) { if (open_iob() == FALSE){ std::cerr << "open_iob: failed to open" << std::endl; @@ -35,7 +43,10 @@ namespace RTC int nsubstep = number_of_substeps(); set_signal_period(period_nsec/nsubstep); std::cout << "period = " << get_signal_period()*nsubstep/1e6 - << "[ms], priority = " << m_priority << std::endl; + << "[ms], priority = " << m_priority << ", cpu = " << m_cpu << std::endl; + struct timeval debug_tv1, debug_tv2, debug_tv3, debug_tv4, debug_tv5; + int loop = 0; + int debug_count = 5000.0/(get_signal_period()*nsubstep/1e6); // Loop count for debug print. Once per 5000.0 [ms]. if (!enterRT()){ unlock_iob(); @@ -43,6 +54,8 @@ namespace RTC return 0; } do{ + loop++; + if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv1, NULL); if (!waitForNextPeriod()){ unlock_iob(); close_iob(); @@ -51,7 +64,7 @@ namespace RTC struct timeval tv; gettimeofday(&tv, NULL); if (m_profile.count > 0){ -#define DELTA_SEC(start, end) end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6; +#define DELTA_SEC(start, end) (end.tv_sec - start.tv_sec + (end.tv_usec - start.tv_usec)/1e6) double dt = DELTA_SEC(m_tv, tv); if (dt > m_profile.max_period) m_profile.max_period = dt; if (dt < m_profile.min_period) m_profile.min_period = dt; @@ -59,6 +72,7 @@ namespace RTC } m_profile.count++; m_tv = tv; + if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv2, NULL); #ifndef OPENRTM_VERSION_TRUNK invoke_worker iw; @@ -86,6 +100,11 @@ namespace RTC tbegin = tend; } #endif + if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT && + rtc_names.size() == processes.size()) { + printRTCProcessingTime(processes); + } + if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv3, NULL); gettimeofday(&tv, NULL); double dt = DELTA_SEC(m_tv, tv); @@ -116,24 +135,35 @@ namespace RTC if (dt > period_sec*nsubstep){ m_profile.timeover++; #ifdef NDEBUG - fprintf(stderr, "[%d.%6.6d] Timeover: processing time = %4.2f[ms]\n", + fprintf(stderr, "[hrpEC][%d.%6.6d] Timeover: processing time = %4.2f[ms]\n", tv.tv_sec, tv.tv_usec, dt*1e3); // Update rtc_names only when rtcs length change. if (processes.size() != rtc_names.size()){ rtc_names.clear(); for (unsigned int i=0; i< processes.size(); i++){ +#ifndef OPENRTM_VERSION_TRUNK RTC::RTObject_var rtc = RTC::RTObject::_narrow(m_comps[i]._ref); +#else + RTC::RTObject_var rtc = list[i]; +#endif rtc_names.push_back(std::string(rtc->get_component_profile()->instance_name)); } } - for (unsigned int i=0; i< processes.size(); i++){ - fprintf(stderr, "%s(%4.2f), ", rtc_names[i].c_str(),processes[i]*1e3); - } - fprintf(stderr, "\n"); + printRTCProcessingTime(processes); #endif } #ifndef OPENRTM_VERSION_TRUNK + if (loop % debug_count == 0 && ENABLE_DEBUG_PRINT) { + gettimeofday(&debug_tv4, NULL); + fprintf(stderr, "[hrpEC] [%d.%6.6d] Processing time breakdown : waitForNextPeriod %f[ms], warker (onExecute) %f[ms], ExecutionProfile %f[ms], time from prev cicle %f[ms]\n", + tv.tv_sec, tv.tv_usec, + DELTA_SEC(debug_tv1, debug_tv2)*1e3, + DELTA_SEC(debug_tv2, debug_tv3)*1e3, + DELTA_SEC(debug_tv3, debug_tv4)*1e3, + DELTA_SEC(debug_tv5, debug_tv1)*1e3); + } + if (loop % debug_count == (debug_count-1) && ENABLE_DEBUG_PRINT) gettimeofday(&debug_tv5, NULL); } while (m_running); #else } while (isRunning()); @@ -170,6 +200,12 @@ namespace RTC throw OpenHRP::ExecutionProfileService::ExecutionProfileServiceException("no such component"); } + void hrpExecutionContext::activate () + { + m_thread_pending = true; + PeriodicExecutionContext::activate (); + } + void hrpExecutionContext::resetProfile() { m_profile.max_period = m_profile.avg_period = 0; diff --git a/ec/hrpEC/hrpEC.cpp b/ec/hrpEC/hrpEC.cpp index c8f8f5aefce..17495490e2c 100644 --- a/ec/hrpEC/hrpEC.cpp +++ b/ec/hrpEC/hrpEC.cpp @@ -2,7 +2,7 @@ #include "hrpEC.h" #include -#include "io/iob.h" +#include "hrpsys/io/iob.h" #include @@ -33,7 +33,9 @@ namespace RTC #else : RTC_exp::PeriodicExecutionContext(), #endif - m_priority(49) + m_priority(49), + m_cpu(-1), + m_thread_pending (false) { resetProfile(); rtclog.setName("hrpEC"); @@ -42,6 +44,7 @@ namespace RTC // Priority getProperty(prop, "exec_cxt.periodic.priority", m_priority); getProperty(prop, "exec_cxt.periodic.rtpreempt.priority", m_priority); + getProperty(prop, "exec_cxt.periodic.cpu_affinity", m_cpu); RTC_DEBUG(("Priority: %d", m_priority)); } @@ -74,6 +77,16 @@ namespace RTC perror("mlockall failed"); } } + + if (m_cpu>=0){ + cpu_set_t cpu_set; + CPU_ZERO(&cpu_set); + CPU_SET(m_cpu, &cpu_set); + int result = sched_setaffinity(0, sizeof(cpu_set_t), &cpu_set); + if (result != 0) { + perror("sched_setaffinity():"); + } + } #endif /* Pre-fault our stack */ stack_prefault(); diff --git a/ec/hrpEC/hrpEC.h b/ec/hrpEC/hrpEC.h index 19e916f4f42..d285a72fb51 100644 --- a/ec/hrpEC/hrpEC.h +++ b/ec/hrpEC/hrpEC.h @@ -11,7 +11,11 @@ #include #include -#include "ExecutionProfileService.hh" +#include "hrpsys/idl/ExecutionProfileService.hh" + +#ifdef __QNX__ +using std::fprintf; +#endif namespace RTC { @@ -31,6 +35,7 @@ namespace RTC #ifdef OPENRTM_VERSION_TRUNK virtual void tick(){} #endif + void activate (); OpenHRP::ExecutionProfileService::Profile *getProfile(); OpenHRP::ExecutionProfileService::ComponentProfile getComponentProfile(RTC::LightweightRTObject_ptr obj); @@ -52,10 +57,22 @@ namespace RTC } } } + void printRTCProcessingTime (std::vector& processes) + { + fprintf(stderr, "[hrpEC] "); + for (unsigned int i=0; i< processes.size(); i++){ + fprintf(stderr, "%s(%4.2f), ", rtc_names[i].c_str(),processes[i]*1e3); + } + fprintf(stderr, "[ms]\n"); + }; + int svc_wrapped (void); + OpenHRP::ExecutionProfileService::Profile m_profile; struct timeval m_tv; int m_priority; + int m_cpu; std::vector rtc_names; + volatile bool m_thread_pending; }; }; diff --git a/hrpsys-base.pc.in b/hrpsys-base.pc.in index 49569b77eda..e7bc111d6f3 100644 --- a/hrpsys-base.pc.in +++ b/hrpsys-base.pc.in @@ -2,7 +2,7 @@ prefix=@CMAKE_INSTALL_PREFIX@ exec_prefix=${prefix}/bin libdir=${prefix}/lib idldir=${prefix}/share/hrpsys/idl -includedir=-I${prefix}/include +includedir=${prefix}/include link_shared_files=@PKG_CONF_LINK_SHARED_FILES@ link_static_files=@PKG_CONF_LINK_STATIC_FILES@ link_depend_dirs=@PKG_CONF_LINK_DEPEND_DIRS@ @@ -14,7 +14,7 @@ cflag_options=@PKG_CONF_CXXFLAG_OPTIONS@ Name: hrpsys-base Description: Basic RT components and utilities to control robots using OpenRTM Requires: openhrp3.1 -Version: 3.1 +Version: @CPACK_PACKAGE_VERSION_MAJOR@.@CPACK_PACKAGE_VERSION_MINOR@.@CPACK_PACKAGE_VERSION_PATCH@ Libs: ${link_depend_dirs} -L${libdir} ${link_shared_files} ${link_depend_options} ${link_depend_files} Libs.private: ${link_static_files} -Cflags: ${cflag_defs} ${cflag_options} ${includedir} +Cflags: ${cflag_defs} ${cflag_options} -I${includedir} diff --git a/idl/AccelerationFilterService.idl b/idl/AccelerationFilterService.idl new file mode 100644 index 00000000000..4b6b1a0088b --- /dev/null +++ b/idl/AccelerationFilterService.idl @@ -0,0 +1,51 @@ +/** + * @file AccelerationFilterService.idl + * @brief Services for the acceleration-filter interface + */ +module OpenHRP +{ + interface AccelerationFilterService + { + typedef double DblArray3[3]; + /** + * @enum ControlMode + * @brief Mode of control + */ + enum ControlMode { + MODE_IDLE, + MODE_ZERO_VELOCITY, + MODE_RELATIVE_LOCAL_VELOCITY, + MODE_ABSOLUTE_LOCAL_VELOCITY, + MODE_RELATIVE_GLOBAL_VELOCITY, + MODE_ABSOLUTE_GLOBAL_VELOCITY + }; + + struct AccelerationFilterParam + { + // + double gravity; + boolean use_filter; + sequence filter_param; + }; + + /** + * @brief Set AccelerationFilter parameters + * @param i_param is input parameter + * @return true if set successfully, false otherwise + */ + boolean setAccelerationFilterParam(in AccelerationFilterParam i_param); + + /** + * @brief Get AccelerationFilter parameters + * @param i_param is input parameter + * @return true if set successfully, false otherwise + */ + boolean getAccelerationFilterParam(out AccelerationFilterParam i_param); + + /** + * @brief reset filter + * @return true if set successfully, false otherwise + */ + boolean resetFilter(in ControlMode mode, in DblArray3 vel); + }; +}; diff --git a/idl/AutoBalancerService.idl b/idl/AutoBalancerService.idl index ccbc974b5bc..96a09f90222 100644 --- a/idl/AutoBalancerService.idl +++ b/idl/AutoBalancerService.idl @@ -3,6 +3,7 @@ * @brief Services for the autobalancer interface */ //#include "OpenHRPCommon.idl" +#include "RobotHardwareService.idl" module OpenHRP { @@ -10,8 +11,11 @@ module OpenHRP { typedef sequence DblSequence3; //typedef sequence DblSequence4; + typedef double DblArray2[2]; typedef double DblArray3[3]; typedef double DblArray4[4]; + typedef double DblArray5[5]; + typedef sequence BoolSequence; /** * @struct Footstep @@ -61,12 +65,33 @@ module OpenHRP sequence sps; }; + /** + * @struct WheelParam + * @brief Wheel parameter for one step + */ + struct WheelParam + { + double goal_pos; + double rv_max; + double ra_max; + }; + + /** + * @struct StepParams + * @brief Step parameters for multi step + */ + struct WheelParams + { + sequence wps; + }; + /** * @struct FootstepSequence * @brief Sequence of foot step. */ typedef sequence FootstepSequence; typedef sequence StepParamSequence; + typedef sequence WheelParamSequence; /** * @struct FootstepsSequence @@ -74,6 +99,7 @@ module OpenHRP */ typedef sequence FootstepsSequence; typedef sequence StepParamsSequence; + typedef sequence WheelParamsSequence; /** * @enum SupportLegState @@ -119,7 +145,12 @@ module OpenHRP MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, - MODE_SYNC_TO_ABC + MODE_SYNC_TO_ABC, + MODE_STIDLE, + MODE_AIR, + MODE_ST, + MODE_SYNC_TO_STIDLE, + MODE_SYNC_TO_AIR }; /** @@ -127,8 +158,24 @@ module OpenHRP * @brief Mode of use_force */ enum UseForceMode { + /// Mode in which robot's COG is not changed according to force/moment values MODE_NO_FORCE, - MODE_REF_FORCE + /// Mode in which robot's COG is changed according to sensors' force/moment values + MODE_REF_FORCE, + /// Mode in which robot's COG is changed according to sensors' force/moment values for hands and feet (and root-link) + MODE_REF_FORCE_WITH_FOOT, + /// Mode in which robot's COG is changed according to sensors' force/moment values using ReferenceForceUpdater's ext moment + MODE_REF_FORCE_RFU_EXT_MOMENT, + MODE_ADDITIONAL_FORCE + }; + + /** + * @enum StrideLimitationType + * @brief Type of stride limitation + */ + enum StrideLimitationType { + SQUARE, + CIRCLE }; /** @@ -155,6 +202,45 @@ module OpenHRP typedef sequence StrSequence; + /** + * @enum STAlgorithm + * @brief Algorithm of Stabilizer + */ + enum STAlgorithm { + TPCC, + EEFM, + EEFMQP, + EEFMQPCOP, + EEFMQPCOP2 + }; + + /** + * @enum EmergencyCheckMode + * @brief Mode of emergency checking + */ + enum EmergencyCheckMode { + NO_CHECK, + COP, + CP, + TILT + }; + + /** + * @enum TwoDimensionVertex + * @brief 2D vertex of a support polygon [m] + */ + struct TwoDimensionVertex { + DblArray2 pos; + }; + + /** + * @enum SupportPolygonVertices + * @brief Vertices of a support polygon at the end effector + */ + struct SupportPolygonVertices { + sequence vertices; + }; + /** * @struct GaitGeneratorParam * @brief Parameters for GaitGenerator. @@ -181,8 +267,10 @@ module OpenHRP double default_double_support_ratio_swing_before; /// Ratio of double support period after swing. Ratio is included in (0, 1). double default_double_support_ratio_swing_after; - /// Stride limitation of forward x[m], y[m], theta[deg], and backward x[m] for goPos - sequence stride_parameter; + /// Stride limitation of forward x[m], outside y[m], outside theta[deg], backward x[m], inside y[m], inside theta[deg] for goPos and goVelocity + sequence stride_parameter; + /// Stride limitation when generating footsteps with stride limitation type CIRCLE (forward, outside, theta, backward, inside) [m] + DblArray5 stride_limitation_for_circle_type; /// Default OrbitType OrbitType default_orbit_type; /// Time offset [s] for swing trajectory by delay_hoffarbib_trajectory_generator. @@ -191,8 +279,14 @@ module OpenHRP double swing_trajectory_final_distance_weight; /// Way point offset 3D vector [m] for stair_delay_hoffarbib_trajectory_generator. DblArray3 stair_trajectory_way_point_offset; + /// Way point offset 3D vector [m] for rectangle_delay_hoffarbib_trajectory_generator. + DblArray3 rectangle_trajectory_way_point_offset; + /// Goal offset 3D vector [m] for rectangle_delay_hoffarbib_trajectory_generator. + DblArray3 rectangle_goal_off; /// Kick point offset 3D vector [m] for cycloid_delay_kick_hoffarbib_trajectory_generator. DblArray3 cycloid_delay_kick_point_offset; + /// Time offset between Z convergence to antecedent path and XY convergence. [swing time]=[Z convergence time]=[XY convergence time]-[swing_trajectory_xy_time_offset]. 0 by default, this means Z convergence time and XY convergence time are same. + double swing_trajectory_time_offset_xy2z; /// Gravitational acceleration [m/s^2] double gravitational_acceleration; /// Toe position offset [m] in end-effector frame x axis @@ -207,12 +301,18 @@ module OpenHRP double toe_angle; /// Maximum heel angle [deg] for heel-contact motion double heel_angle; + /// Threshould [m] (>=0) whether toe is used or not. This is used only if use_toe_heel_auto_set == true. + double toe_check_thre; + /// Threshould [m] (>=0) whether heel is used or not. This is used only if use_toe_heel_auto_set == true. + double heel_check_thre; /// Sequence of phase ratio of toe-off and heel-contact. Sum of toe_heel_phase_ratio should be 1.0. sequence toe_heel_phase_ratio; /// Use toe joint or not in toe-off heel-contact motion. boolean use_toe_joint; /// Use toe heel zmp transition. If true, zmp moves among default position, toe position (described by toe_zmp_offset_x), and heel position (described by heel_zmp_offset_x). boolean use_toe_heel_transition; + /// Use toe heel auto setting. If true, gait generator autonomously determine whether toe and heel are used. + boolean use_toe_heel_auto_set; /// ZMP weight of RLEG, LLEG, RARM and LARM sequence zmp_weight_map; /// Foot position offset[m] (rleg and lleg) @@ -221,6 +321,60 @@ module OpenHRP long optional_go_pos_finalize_footstep_num; /// Offset for overwritable footstep index. Offset from current footstep index. Used in emergency_stop and velocity_mode. long overwritable_footstep_index_offset; + /// Stride limitation when overwriting footsteps (forward, outside, theta, backward, inside) [m] + DblArray5 overwritable_stride_limitation; + /// Use stride limitation or not + boolean use_stride_limitation; + /// Stride limitation type + StrideLimitationType stride_limitation_type; + /// Leg margin between foot end effector position and foot edge (front, rear, outside, inside) [m] + DblArray4 leg_margin; + /// Safe leg margin between foot end effector position and foot edge (front, rear, outside, inside) [m] + DblArray4 safe_leg_margin; + /// Feedback gain when modifying footsteps based on Capture Point + double footstep_modification_gain; + /// Whether modify footsteps based on Capture Point + boolean modify_footsteps; + /// CP check margin when modifying footsteps (x, y) [m] + DblArray2 cp_check_margin; + /// Margin time ratio for footstep modification before landing [s] + double margin_time_ratio; + /// How much act_vel is used for walking pattern generation (0.0 ~ 1.0) + double act_vel_ratio; + /// overwritable min time until landing for fg[s] + double min_time_mgn; + /// overwritable min step time for fg[s] + double min_time; + /// Whether use disturbance compensation or not + boolean use_disturbance_compensation; + /// Gain for disturbance compensation + double dc_gain; + /// Offset of y component of ref_dcm + double dcm_offset; + /// Step time when emergency stepping [s] + DblArray3 emergency_step_time; + /// Whether use act states + boolean use_act_states; + /// zmp delay time [s] + double zmp_delay_time_const; + double overwritable_max_time; + double fg_zmp_cutoff_freq; + double fg_cp_cutoff_freq; + DblArray3 sum_d_footstep_thre; + DblArray3 footstep_check_delta; + boolean is_interpolate_zmp_in_double; + /// whether gostop after becoming stable + boolean is_stable_go_stop_mode; + boolean use_flywheel_balance; + boolean debug_set_landing_height; + double debug_landing_height; + DblArray2 debug_landing_height_xrange; + // (x, y) [m] + DblArray2 front_edge_offset_of_steppable_region; + boolean is_slow_stair_mode; + double stair_step_time; + long num_preview_step; + double rectangle_time_smooth_offset; }; /** @@ -238,8 +392,32 @@ module OpenHRP double reference_gain; /// Manipulability limit for inverse kinematics. double manipulability_limit; + /// IK loop count + unsigned short ik_loop_count; + }; + + /** + * @struct JointServoControlParameter + * @brief Joint PD Servo Control Parameters + */ + struct JointServoControlParameter { + sequence support_pgain; + sequence support_dgain; + sequence landing_pgain; + sequence landing_dgain; + sequence swing_pgain; + sequence swing_dgain; }; + /** + * @enum IKMode + * @brief Inverse Kinematics mode + */ + enum IKMode { + SIMPLE, + FULLBODY + }; + /** * @struct AutoBalancerParam * @brief Parameters for AutoBalancer @@ -276,6 +454,202 @@ module OpenHRP GaitType default_gait_type; /// Sequence for all end-effectors' ik limb parameters sequence ik_limb_parameters; + /// Whether change root link height for avoiding limb stretch + boolean use_limb_stretch_avoidance; + /// Limb stretch avoidance time constant [s] + double limb_stretch_avoidance_time_const; + /// Root link height change limitation for avoiding limb stretch [m/s] (lower, upper) + DblArray2 limb_stretch_avoidance_vlimit; + /// Sequence of limb length margin from max limb length [m] + sequence limb_length_margin; + /// Name of additional force applied link for MODE_REF_FORCE_WITH_FOOT for MODE_REF_FORCE_RFU_EXT_MOMENT. Additional force is applied to the point without force sensors, such as torso. + string additional_force_applied_link_name; + /// Link local offset[m] of force applied point for MODE_REF_FORCE_WITH_FOOT. This is local value in the link frame specified by "additional_force_applied_link_name". Zero by default. + DblArray3 additional_force_applied_point_offset; + /// switch SimpleFullbodyIK to FullbodyIK + IKMode ik_mode; + /// whether step when emergency + boolean is_emergency_step_mode; + /// whether touch wall when emergency + boolean is_emergency_touch_wall_mode; + /// whether use collision avoidance + boolean use_collision_avoidance; + /// cog height constraint in fullbody IK + double cog_z_constraint; + /// Sync time for retrieve touch wall motion [s] + double touch_wall_retrieve_time; + boolean is_natural_walk; + boolean is_stop_early_foot; + double arm_swing_deg; + boolean debug_read_steppable_region; + long ik_max_loop; + }; + + /** + * @struct StabilizerParam + * @brief Stabilizer Parameters. + */ + struct StabilizerParam { + // for TPCC + /// Feedback gain for ZMP tracking error (x,y) + DblArray2 k_tpcc_p; + /// Feedback gain for COG position tracking error (x,y) + DblArray2 k_tpcc_x; + /// Body posture control gain [rad/s] (roll, pitch). + DblArray2 k_brot_p; + /// Time constant for body posture control [s] (roll, pitch). + DblArray2 k_brot_tc; + // // for RUNST + // DblArray2 k_run_b; + // DblArray2 d_run_b; + // DblArray2 tdfke; + // DblArray2 tdftc; + // double k_run_x; + // double k_run_y; + // double d_run_x; + // double d_run_y; + // for EEFM ST + /// Feedback gain for COG position tracking error (x,y) + DblArray2 eefm_k1; + /// Feedback gain for COG velocity tracking error (x,y) + DblArray2 eefm_k2; + /// Feedback gain for ZMP tracking error (x,y) + DblArray2 eefm_k3; + /// Time constant for stabilizer ZMP delay [s] (x,y) + DblArray2 eefm_zmp_delay_time_const; + /// Auxiliary input for ZMP position [m] (x,y). This is used for delay model identification + DblArray2 eefm_ref_zmp_aux; + /// Sequence of all end-effector rotation damping gain [Nm/(rad/s)] (r,p,y). + sequence > eefm_rot_damping_gain; + /// Sequence of all end-effector rotation damping time constant [s] (r,p,y). + sequence > eefm_rot_time_const; + /// Sequence of all end-effector position damping gain [N/(m/s)] (x,y,z). + sequence > eefm_pos_damping_gain; + /// Sequence of all end-effector position damping time constant for double support phase [s] (x,y,z). + sequence > eefm_pos_time_const_support; + /// Sequence of all swing leg end-effector rotation spring gain (r,p,y). + sequence > eefm_swing_rot_spring_gain; + /// Sequence of all swing leg end-effector rotation spring time constant [s] (r,p,y). + sequence > eefm_swing_rot_time_const; + /// Sequence of all swing leg end-effector position spring gain (x,y,z). + sequence > eefm_swing_pos_spring_gain; + /// Sequence of all swing leg end-effector position spring time constant [s] (x,y,z). + sequence > eefm_swing_pos_time_const; + /// Sequence of all end-effector end-effector-frame moment limit [Nm] + sequence > eefm_ee_moment_limit; + /// Sequence of all end-effector position compensation limit [m] + sequence eefm_pos_compensation_limit; + /// Sequence of all end-effector rot compensation limit [rad] + sequence eefm_rot_compensation_limit; + /// End-effector position damping time constant for single support phase [s]. + double eefm_pos_time_const_swing; + /// Transition time for single=>double support phase gain interpolation [s]. + double eefm_pos_transition_time; + /// Margin for transition time for single=>double support phase gain interpolation [s]. + double eefm_pos_margin_time; + /// Inside foot margine [m]. Distance between foot end effector position and foot inside edge. + double eefm_leg_inside_margin; + /// Outside foot margine [m]. Distance between foot end effector position and foot outside edge. + double eefm_leg_outside_margin; + /// Front foot margine [m]. Distance between foot end effector position and foot front edge. + double eefm_leg_front_margin; + /// Rear foot margine [m]. Distance between foot end effector position and foot rear edge. + double eefm_leg_rear_margin; + /// Body attitude control gain [rad/s] (roll, pitch) for EEFM. + DblArray2 eefm_body_attitude_control_gain; + /// Time constant for body attitude control [s] (roll, pitch) for EEFM. + DblArray2 eefm_body_attitude_control_time_const; + /// Cutoff frequency of LPF in calculation of COG velocity [Hz] + double eefm_cogvel_cutoff_freq; + /// Blending parameter [0, 1] for wrench distribution + double eefm_wrench_alpha_blending; + /// Cutoff frequency of LPF in calculation of force moment distribution alpha ratio parameter [Hz] + double eefm_alpha_cutoff_freq; + /// Gravitational acceleration [m/s^2] used in ST calculation + double eefm_gravitational_acceleration; + /// Pos error gain + double eefm_ee_pos_error_p_gain; + /// Rot error gain + double eefm_ee_rot_error_p_gain; + /// Pos rot error cutoff freq [Hz] + double eefm_ee_error_cutoff_freq; + /// Sequence of vertices for all end effectors assuming that the order is (rleg, lleg, rarm, larm) + sequence< SupportPolygonVertices > eefm_support_polygon_vertices_sequence; + /// Use force difference control or each limb force control. True by default. + boolean eefm_use_force_difference_control; + /// Use damping control for swing leg. + boolean eefm_use_swing_damping; + /// Swing damping control force threshold [N] + DblArray3 eefm_swing_damping_force_thre; + /// Swing damping control moment threshold [Nm] + DblArray3 eefm_swing_damping_moment_thre; + /// Rotation damping gain for swing leg [Nm/(rad/s)] (r,p,y) + DblArray3 eefm_swing_rot_damping_gain; + /// Position damping gain for swing leg [N/(m/s)] (x,y,z) + DblArray3 eefm_swing_pos_damping_gain; + /// Sequence of all end-effector force/moment distribution weight + sequence > eefm_ee_forcemoment_distribution_weight; + // common + /// Current Stabilizer algorithm + STAlgorithm st_algorithm; + /// Current ControllerMode + ControllerMode controller_mode; + /// Transition time [s] for start and stop Stabilizer + double transition_time; + /// Bool sequence for all end effectors whether the end effector is used for limb IK. + BoolSequence is_ik_enable; + /// Bool sequence for all end effectors whether the end effector is used for feedback control (currently damping control). + BoolSequence is_feedback_control_enable; + /// Bool sequence for all end effectors whether the end effector is used for zmp calculation. + BoolSequence is_zmp_calc_enable; + /// COP margin [m] from edges for COP checking + double cop_check_margin; + /// CP margin [m] (front, rear, inside, outside) + DblArray4 cp_check_margin; + /// tilt margin [rad] (single support phase, double support phase) from reference floor + DblArray2 tilt_margin; + /// ref_CP [m] (x,y) (foot_origin relative coordinate) + DblArray2 ref_capture_point; + /// act_CP [m] (x,y) (foot_origin relative coordinate) + DblArray2 act_capture_point; + /// CP_offset [m] (x,y) (foot_origin relative coordinate) + DblArray2 cp_offset; + /// contact decision threshold [N] + double contact_decision_threshold; + /// Foot origin position offset + sequence< sequence > foot_origin_offset; + /// Emergency signal checking mode + EmergencyCheckMode emergency_check_mode; + sequence end_effector_list; + /// whether an emergency stop is used while walking + boolean is_estop_while_walking; + /// Sequence for all end-effectors' ik limb parameters + sequence ik_limb_parameters; + /// Whether change root link height for avoiding limb stretch + boolean use_limb_stretch_avoidance; + /// Limb stretch avoidance time constant [s] + double limb_stretch_avoidance_time_const; + /// Root link height change limitation for avoiding limb stretch [m/s] (lower, upper) + DblArray2 limb_stretch_avoidance_vlimit; + /// Sequence of limb length margin from max limb length [m] + sequence limb_length_margin; + /// Detection time whether is in air [s] + double detection_time_to_air; + /// Limit of compensation for difference between ref-act root rot [rad]. + DblArray2 root_rot_compensation_limit; + /// Whether truncate zmp according to foot support polygon or not + boolean use_zmp_truncation; + boolean use_force_sensor; + boolean use_footguided_stabilizer; + double footguided_balance_time_const; + // For joint servo control parameter + RobotHardwareService::JointControlMode joint_control_mode; + double swing2landing_transition_time; + double landing_phase_time; + double landing2support_transition_time; + double support_phase_min_time; + double support2swing_transition_time; + sequence joint_servo_control_parameters; }; /** @@ -285,6 +659,20 @@ module OpenHRP */ boolean goPos(in double x, in double y, in double th); + /** + * @brief + * @param + * @return true if set successfully, false otherwise + */ + boolean goPosWheel(in double x, in double y, in double th, in double w_x, in double rv_max, in double ra_max, in double tm_off); + + /** + * @brief + * @param + * @return true if set successfully, false otherwise + */ + boolean goWheel(in double x, in double rv_max, in double ra_max); + /** * @brief Walk at the desired velocity. If the robot is stopping, the robot starts stepping. Returns without waiting for whole steps to be executed. * @param i_vx[m/s], i_vy[m/s], and i_vth[deg/s] are velocity in the current mid-coords of right foot and left foot. @@ -299,6 +687,8 @@ module OpenHRP */ boolean goStop(); + boolean jumpTo(in double x, in double y, in double z, in double ts, in double tf); + /** * @brief Stop stepping immediately. * @param @@ -322,6 +712,14 @@ module OpenHRP */ boolean setFootStepsWithParam(in FootstepsSequence fss, in StepParamsSequence spss, in long overwrite_fs_idx); + /** + * @brief Set footsteps. Returns without waiting for whole steps to be executed. + * @param fss is sequence of FootStepWithParam structure. + overwrite_fs_idx is index to be overwritten. overwrite_fs_idx is used only in walking. + * @return true if set successfully, false otherwise + */ + boolean setFootStepsWithWheel(in FootstepsSequence fss, in StepParamsSequence spss, in WheelParamsSequence wpss, in long overwrite_fs_idx); + /** * @brief Wait for whole footsteps are executed. * @param @@ -413,5 +811,33 @@ module OpenHRP * @return true if set successfully, false otherwise */ boolean releaseEmergencyStop(); + + /** + * @brief Get Stabilizer parameters. + * @param i_param is output parameters + * @return true if set successfully, false otherwise + */ + void getStabilizerParam(out StabilizerParam i_param); + + /** + * @brief Set Stabilizer parameters. + * @param i_param is input parameters + * @return true if set successfully, false otherwise + */ + void setStabilizerParam(in StabilizerParam i_param); + + /** + * @brief Start Stabilizer mode. + * @param + * @return + */ + void startStabilizer(); + + /** + * @brief Sop Stabilizer mode. + * @param + * @return + */ + void stopStabilizer(); }; }; diff --git a/idl/CMakeLists.txt b/idl/CMakeLists.txt index 963cec2f232..08d5f23bdf6 100644 --- a/idl/CMakeLists.txt +++ b/idl/CMakeLists.txt @@ -26,6 +26,10 @@ set(idl_files ServoControllerService.idl TorqueControllerService.idl ThermoLimiterService.idl + ReferenceForceUpdaterService.idl + AccelerationFilterService.idl + ObjectContactTurnaroundDetectorService.idl + PCDLoaderService.idl ) set(openhrp_idl_files @@ -104,7 +108,7 @@ add_library(${target} SHARED ${sources}) target_link_libraries(${target} ${OPENRTM_LIBRARIES}) install(TARGETS ${target} - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + LIBRARY DESTINATION lib ) if(COMPILE_JAVA_STUFF) diff --git a/idl/CollisionDetectorService.idl b/idl/CollisionDetectorService.idl index 993d54b4052..669a9844171 100644 --- a/idl/CollisionDetectorService.idl +++ b/idl/CollisionDetectorService.idl @@ -21,7 +21,7 @@ module OpenHRP /** * @brief set tolerance value for collision detector * @param link_pair_name name of link pair - * @param tolerance tolerance + * @param tolerance tolerance [m] * @return true if set successfully, false otherwise */ boolean setTolerance(in string link_pair_name, in double tolerance); @@ -46,5 +46,12 @@ module OpenHRP sequence lines; }; boolean getCollisionStatus(out CollisionState cs); + + /** + * @brief set collision loop for collision detector + * @param loop for loop integer + * @return true if set successfully, false otherwise + */ + boolean setCollisionLoop(in short loop); }; }; diff --git a/idl/EmergencyStopperService.idl b/idl/EmergencyStopperService.idl index e1907273202..b3fc1222d96 100644 --- a/idl/EmergencyStopperService.idl +++ b/idl/EmergencyStopperService.idl @@ -5,6 +5,8 @@ module OpenHRP { interface EmergencyStopperService { + typedef sequence dSequence; + /** * @struct EmergencyStopperParam * @brief Parameters for EmergencyStopper. @@ -15,6 +17,8 @@ module OpenHRP double default_recover_time; /// Sync time for retrieve motion [s] double default_retrieve_time; + /// duration for retrieve motion [s] + double default_retrieve_duration; /// Stop mode or not. boolean is_stop_mode; }; @@ -42,7 +46,13 @@ module OpenHRP * @return true if set successfully, false otherwise */ boolean setEmergencyStopperParam(in EmergencyStopperParam i_param); + + /** + * @brief Set EmergencyStopperParam + * @return true if set successfully, false otherwise + */ + boolean setEmergencyJointAngles(in dSequence angles, in boolean solved); }; }; -#endif \ No newline at end of file +#endif diff --git a/idl/HRPDataTypes.idl b/idl/HRPDataTypes.idl index 9b0783319a5..3e246ed73b9 100644 --- a/idl/HRPDataTypes.idl +++ b/idl/HRPDataTypes.idl @@ -62,6 +62,34 @@ module OpenHRP RTC::Velocity3D vel; }; + struct LandingPosition + { + double x; + double y; + double z; + double nx; + double ny; + double nz; + long l_r; // 0: right. 1: left. + }; + + struct SteppableRegion + { + sequence > region; + long l_r; // 0: right. 1: left. + }; + + struct CogState + { + double x; + double y; + double z; + double vx; + double vy; + double vz; + long l_r; // 0: right. 1: left. + }; + //------------------------------------------------------------ // Timed data types //------------------------------------------------------------ @@ -83,4 +111,22 @@ module OpenHRP RTC::Time tm; SpatialVelocity data; }; + + struct TimedLandingPosition + { + RTC::Time tm; + LandingPosition data; + }; + + struct TimedSteppableRegion + { + RTC::Time tm; + SteppableRegion data; + }; + + struct TimedCogState + { + RTC::Time tm; + CogState data; + }; }; diff --git a/idl/Img.idl b/idl/Img.idl index 75f8a20e518..d538008aeac 100644 --- a/idl/Img.idl +++ b/idl/Img.idl @@ -15,7 +15,7 @@ typedef double Mat44[4][4]; enum ColorFormat { CF_UNKNOWN, CF_GRAY, CF_RGB, - CF_GRAY_JPEG, CF_RGB_JPEG // local extension + CF_GRAY_JPEG, CF_RGB_JPEG, CF_DEPTH // local extension }; struct ImageData diff --git a/idl/ImpedanceControllerService.idl b/idl/ImpedanceControllerService.idl index ad9bbe4f552..89462ad2f71 100644 --- a/idl/ImpedanceControllerService.idl +++ b/idl/ImpedanceControllerService.idl @@ -59,51 +59,6 @@ module OpenHRP boolean use_sh_base_pos_rpy; }; - /** - * @enum DetectorMode - * @brief Mode of object turnaround detector - */ - enum DetectorMode { - MODE_DETECTOR_IDLE, - MODE_STARTED, - MODE_DETECTED, - MODE_MAX_TIME - }; - - /** - * @enum DetectorTotalWrench - * @brief Flag for whether total force or total moment is checked. - */ - enum DetectorTotalWrench { - TOTAL_FORCE, - TOTAL_MOMENT - }; - - /** - * @struct objectTurnaroundDetectorParam - * @brief ObjectTurnaroundDetectorParam parameters - */ - struct objectTurnaroundDetectorParam { - /// Cutoff frequence for wrench value [Hz] - double wrench_cutoff_freq; - /// Cutoff frequence for dwrench value [Hz] - double dwrench_cutoff_freq; - /// Threshould for detection (0,1) - double detect_ratio_thre; - /// Threshould for starting detection (0,1) - double start_ratio_thre; - /// Threshould for time [s] after the first object turnaround detection (Wait detect_time_thre [s] after first object turnaround detection). - double detect_time_thre; - /// Threshould for time [s] after the first starting detection (Wait start_time_thre [s] after first start detection). - double start_time_thre; - /// Axis - DblArray3 axis; - /// Moment center [m] from foot mid frame (middle frame of rleg and lleg) - DblArray3 moment_center; - /// Current flag for whether total force or total moment is checked. - DetectorTotalWrench detector_total_wrench; - }; - /** * @brief start impedance controller with waiting for transition. * @param name impedance controller param's name, which basically corresponds to force sensor name @@ -153,39 +108,5 @@ module OpenHRP * @param name impedance controller param's name, which basically corresponds to force sensor name */ void waitImpedanceControllerTransition(in string name); - - /** - * @brief Start ObjectTurnaroundDetector. - * @param ref_diff_wrench is force or moment value. - * @param max_time is max time [s]. - * @param i_ee_names is list of end effector - */ - void startObjectTurnaroundDetection(in double i_ref_diff_wrench, in double i_max_time, in StrSequence i_ee_names); - - /** - * @brief Check ObjectTurnaroundDetector. - */ - DetectorMode checkObjectTurnaroundDetection(); - - /** - * @brief set object turnaround detector parameters. - * @param i_param input new parameters - * @return true if set successfully, false otherwise - */ - boolean setObjectTurnaroundDetectorParam(in objectTurnaroundDetectorParam i_param); - - /** - * @brief get object turnaround detector parameters. - * @param i_param input new parameters - * @return true if set successfully, false otherwise - */ - boolean getObjectTurnaroundDetectorParam(out objectTurnaroundDetectorParam i_param); - - /** - * @brief get Current force and moment for object - * @param i_param output force and moment - * @return true if set successfully, false otherwise - */ - boolean getObjectForcesMoments(out Dbl3Sequence o_forces, out Dbl3Sequence o_moments, out DblSequence3 o_3dofwrench); }; }; diff --git a/idl/ObjectContactTurnaroundDetectorService.idl b/idl/ObjectContactTurnaroundDetectorService.idl new file mode 100644 index 00000000000..1e5f2c6bc9a --- /dev/null +++ b/idl/ObjectContactTurnaroundDetectorService.idl @@ -0,0 +1,150 @@ +/** + * @file ObjectContactTurnaroundDetectorService.idl + * @brief Services for the object contact turnaround detector interface + */ +module OpenHRP +{ + + interface ObjectContactTurnaroundDetectorService + { + typedef sequence DblSequence3; + typedef double DblArray3[3]; + typedef double DblArray6[6]; + typedef sequence StrSequence; + typedef sequence > Dbl3Sequence; + typedef sequence DblSequence; + + /** + * @enum DetectorMode + * @brief Mode of object contact turnaround detector + */ + enum DetectorMode { + MODE_DETECTOR_IDLE, + MODE_STARTED, + MODE_DETECTED, + MODE_MAX_TIME, + MODE_OTHER_DETECTED + }; + typedef sequence DetectorModeSequence; + + /** + * @enum DetectorTotalWrench + * @brief Flag for whether total force or total moment is checked. + */ + enum DetectorTotalWrench { + /// Check resultant force + TOTAL_FORCE, + /// Check resultant moment using force measurements + TOTAL_MOMENT, + /// Check resultant moment using force and moment measurements + TOTAL_MOMENT2, + /// Check generalized wrench + GENERALIZED_WRENCH + }; + + /** + * @struct objectContactTurnaroundDetectorParam + * @brief ObjectContactTurnaroundDetectorParam parameters + */ + struct objectContactTurnaroundDetectorParam { + /// Cutoff frequence for wrench value [Hz] + double wrench_cutoff_freq; + /// Cutoff frequence for dwrench value [Hz] + double dwrench_cutoff_freq; + /// Threshould for detection (0,1) + double detect_ratio_thre; + /// Threshould for starting detection (0,1) + double start_ratio_thre; + /// Threshould for time [s] after the first object contact turnaround detection (Wait detect_time_thre [s] after first object contact turnaround detection). + double detect_time_thre; + /// Threshould for time [s] after the first starting detection (Wait start_time_thre [s] after first start detection). + double start_time_thre; + /// Axis + DblArray3 axis; + /// Moment center [m] from foot mid frame (middle frame of rleg and lleg) + DblArray3 moment_center; + /// Current flag for whether total force or total moment is checked. + DetectorTotalWrench detector_total_wrench; + /// Constraint conversion matrix 1 (Nx6 vector <= N x 6 matrix) : not for friction coefficient + DblSequence constraint_conversion_matrix1; + /// Constraint conversion matrix 2 (Nx6 vector <= N x 6 matrix) : for friction coefficient + DblSequence constraint_conversion_matrix2; + /// Hold values which can be obtained from idl functions when turnaround is detected. False by default (for backward compatibility). + boolean is_hold_values; + /// Reference velocity of wrenches (reference velocity of phi1). Used for GENERALIZED_WRENCH + DblSequence ref_dphi1; + /// List of end effector names. Used for GENERALIZED_WRENCH + StrSequence limbs; + /// Max time. Used for GENERALIZED_WRENCH + double max_time; + /// Threshould for time [s] to move to MODE_OTHER_DETECTED after the first MODE_DETECTED, that is, do not check contact change other than detected element. + double other_detect_time_thre; + /// Threshould for forgetting while MODE_STARTED (until MODE_DETECTED). + double forgetting_ratio_thre; + }; + + /** + * @struct objectGeneralizedConstraintWrenchesParam + * @brief for getObjectGeneralizedConstraintWrenches argument + */ + struct objectGeneralizedConstraintWrenchesParam { + /// Filtered generalized constraint wrench1 (not for friction coefficient). + DblSequence generalized_constraint_wrench1; + /// Filtered generalized constraint wrench2 (for friction coefficient). + DblSequence generalized_constraint_wrench2; + /// Filtered resultant wrench. + DblArray6 resultant_wrench; + }; + + /** + * @brief Start ObjectContactTurnaroundDetector. + * @param ref_diff_wrench is force or moment value. + * @param max_time is max time [s]. + * @param i_ee_names is list of end effector + */ + void startObjectContactTurnaroundDetection(in double i_ref_diff_wrench, in double i_max_time, in StrSequence i_ee_names); + + /** + * @brief Check ObjectContactTurnaroundDetector. + */ + DetectorMode checkObjectContactTurnaroundDetection(); + + /** + * @brief set object contact turnaround detector parameters. + * @param i_param input new parameters + * @return true if set successfully, false otherwise + */ + boolean setObjectContactTurnaroundDetectorParam(in objectContactTurnaroundDetectorParam i_param); + + /** + * @brief get object contact turnaround detector parameters. + * @param i_param input new parameters + * @return true if set successfully, false otherwise + */ + boolean getObjectContactTurnaroundDetectorParam(out objectContactTurnaroundDetectorParam i_param); + + /** + * @brief get Current force and moment for object + * @param o_forces and o_moments are list of forces[N] and moments[Nm] at end-effectors. o_3dofwrench is resultant force[N] or wrench[Nm]. o_fric_coeff_wrench is firction coefficient wrench[N or Nm]. + * @return true if set successfully, false otherwise + */ + boolean getObjectForcesMoments(out Dbl3Sequence o_forces, out Dbl3Sequence o_moments, out DblSequence3 o_3dofwrench, out double o_fric_coeff_wrench); + + /** + * @brief Check ObjectContactTurnaroundDetector for GeneralizedWrench mode. + */ + boolean checkObjectContactTurnaroundDetectionForGeneralizedWrench(out DetectorModeSequence o_dms); + + /** + * @brief Start ObjectContactTurnaroundDetector for GeneralizedWrench mode. + */ + boolean startObjectContactTurnaroundDetectionForGeneralizedWrench(); + + /** + * @brief get Current generalized constraint wrenches for Generalized Wrench mode. + * @param o_param output parameters + * @return true if set successfully, false otherwise + */ + boolean getObjectGeneralizedConstraintWrenches(out objectGeneralizedConstraintWrenchesParam o_param); + }; +}; diff --git a/idl/PCDLoaderService.idl b/idl/PCDLoaderService.idl new file mode 100644 index 00000000000..b1efa1d69dc --- /dev/null +++ b/idl/PCDLoaderService.idl @@ -0,0 +1,28 @@ +/** + * @file PCDLoaderService.idl + * @brief Services for the pcd loader interface + */ +#include "ExtendedDataTypes.idl" + +module OpenHRP +{ + struct PCDOffset + { + string label; + RTC::Pose3D data; + RTC::Point3D center; + }; + + typedef sequence PCDOffsetSeq; + + interface PCDLoaderService + { + /** + @brief load pcd file + @param filename file name of pcd + @param label in order to select pcd when multiple pcds are loaded + @return true if loaded successfully, false otherwise + */ + boolean load(in string filename, in string label); + }; +}; diff --git a/idl/ReferenceForceUpdaterService.idl b/idl/ReferenceForceUpdaterService.idl new file mode 100644 index 00000000000..2da20e753c5 --- /dev/null +++ b/idl/ReferenceForceUpdaterService.idl @@ -0,0 +1,91 @@ +/** + * @file ReferenceForceUpdaterService.idl + * @brief Services for the reference force updater interface + */ +module OpenHRP +{ + interface ReferenceForceUpdaterService + { + typedef sequence DblSequence3; + typedef sequence StrSequence; + + /** + * @struct ReferenceForceUpdaterParam + * @brief ReferenceForceUpdater Parameters. + */ + struct ReferenceForceUpdaterParam + { + /// Motion direction to update reference force + DblSequence3 motion_dir; + /// frame of motion_dir ("local" or "world"). + /// "local" is coordinate at end effector, and "world" is the coordinate between the feet. + string frame; + /// Update frequency [Hz] + double update_freq; + /// Update time ratio \in [0,1] + double update_time_ratio; + /// P gain + double p_gain; + /// D gain + double d_gain; + /// I gain + double i_gain; + /// Hold current value or not. If is_hold_value is true, do not update value, otherwise, update value. + boolean is_hold_value; + /// Transition time[s] + double transition_time; + /// Cutoff frequency for actual force value [Hz] + double cutoff_freq; + }; + + /** + * @brief Set ReferenceForceUpdater parameters + * @param i_param is input parameter + * @return true if set successfully, false otherwise + */ + boolean setReferenceForceUpdaterParam(in string name, in ReferenceForceUpdaterParam i_param); + + /** + * @brief Get ReferenceForceUpdater parameters + * @param i_param is input parameter + * @return true if set successfully, false otherwise + */ + boolean getReferenceForceUpdaterParam(in string name, out ReferenceForceUpdaterParam i_param); + + /** + * @brief Start ReferenceForceUpdater + * @return true if set successfully, false otherwise + */ + boolean startReferenceForceUpdater(in string name); + + /** + * @brief Stop ReferenceForceUpdater + * @return true if set successfully, false otherwise + */ + boolean stopReferenceForceUpdater(in string name); + + /** + * @brief Start ReferenceForceUpdater and function return immediately + * @return true if set successfully, false otherwise + */ + boolean startReferenceForceUpdaterNoWait(in string name); + + /** + * @brief Stop ReferenceForceUpdater and function return immediately + * @return true if set successfully, false otherwise + */ + boolean stopReferenceForceUpdaterNoWait(in string name); + + /** + * @brief Wait for ReferenceForceUpdater mode transition. + */ + void waitReferenceForceUpdaterTransition(in string name); + + /** + * @brief Get supported name sequence of ReferenceForceUpdater + * @param o_param is supported name sequence + * @return true if set successfully, false otherwise + */ + boolean getSupportedReferenceForceUpdaterNameSequence(out StrSequence o_names); + }; +}; diff --git a/idl/RemoveForceSensorLinkOffsetService.idl b/idl/RemoveForceSensorLinkOffsetService.idl index 3aba05e9d8e..10f308de46d 100644 --- a/idl/RemoveForceSensorLinkOffsetService.idl +++ b/idl/RemoveForceSensorLinkOffsetService.idl @@ -11,6 +11,7 @@ module OpenHRP * @brief Parameters for a link mass and center of mass. The link is assumed to be end-effector-side from force-moment sensor. */ typedef sequence DblSequence3; + typedef sequence StrSequence; struct forcemomentOffsetParam { /// Force offset [N]. DblSequence3 force_offset; @@ -49,5 +50,13 @@ module OpenHRP * @return true if dump successfully, false otherwise */ boolean dumpForceMomentOffsetParams(in string filename); + + /** + * @brief remove offsets on sensor outputs form force/torque sensors. Sensor offsets (force_offset and moment_offset in ForceMomentOffsetParam) are calibrated. This function takes several time (for example 8.0[s]). Please keep the robot static and make sure that robot's sensors do not contact with any objects. + * @param names is list of sensor names to be calibrated. If not specified, all sensors are calibrated by default. + * @param tm is calibration time[s]. + * @return true if set successfully, false otherwise + */ + boolean removeForceSensorOffset(in StrSequence names, in double tm); }; }; diff --git a/idl/RobotHardwareService.idl b/idl/RobotHardwareService.idl index 22a2a007acf..b083730e9ff 100644 --- a/idl/RobotHardwareService.idl +++ b/idl/RobotHardwareService.idl @@ -2,6 +2,9 @@ * @file RobotHardwareService.idl * @brief Services for the robot hardware interface */ + +#include "BasicDataType.idl" + module OpenHRP { interface RobotHardwareService @@ -89,6 +92,36 @@ module OpenHRP */ void setServoGainPercentage(in string name, in double percentage); + /** + * @brief set the parcentage to the default servo Pgain + * @param name joint name, part name or "all" + * @param percentage to joint servo gain[0-100] + */ + void setServoPGainPercentage(in string name, in double percentage); + + /** + * @brief set the parcentage to the default servo Dgain + * @param name joint name, part name or "all" + * @param percentage to joint servo gain[0-100] + */ + void setServoDGainPercentage(in string name, in double percentage); + + /** + * @brief set the parcentage to the default servo Pgain with traisition time + * @param name joint name, part name or "all" + * @param percentage to joint servo gain[0-100] + * @param length of time [s] required to change gain + */ + void setServoPGainPercentageWithTime(in string name, in double percentage, in double time); + + /** + * @brief set the parcentage to the default servo Dgain with transition time + * @param name joint name, part name or "all" + * @param percentage to joint servo gain[0-100] + * @param length of time [s] required to change gain + */ + void setServoDGainPercentageWithTime(in string name, in double percentage, in double time); + /** * @brief set the maximum joint servo error angle * @param name joint name, part name or "all" @@ -97,12 +130,12 @@ module OpenHRP void setServoErrorLimit(in string name, in double limit); /** - * @brief remove offsets on sensor outputs form gyro sensors and accelerometers + * @brief remove offsets on sensor outputs form gyro sensors and accelerometers. This function takes 10[s]. Please keep the robot static. */ void calibrateInertiaSensor(); /** - * @brief remove offsets on sensor outputs form force/torque sensors + * @brief remove offsets on sensor outputs form force/torque sensors. This function takes 10[s]. Please keep the robot static and make sure that robot's sensors do not contact with any objects. */ void removeForceSensorOffset(); @@ -205,5 +238,59 @@ module OpenHRP */ void getStatus2(out RobotState2 rs); + /** + * @brief RobotState2 with timestamp + */ + struct TimedRobotState2 + { + RTC::Time tm; + RobotState2 data; + }; + + /** + * @brief set joint inertia + * @param name joint name + * @param mn joint inertia + * @return true if set successfully, false otherwise + */ + boolean setJointInertia(in string name, in double mn); + + /** + * @brief set joint inertias + * @param mns array of joint inertia + */ + void setJointInertias(in DblSequence mns); + + /** + * @brief enable disturbance observer + */ + void enableDisturbanceObserver(); + + /** + * @brief disable disturbance observer + */ + void disableDisturbanceObserver(); + + /** + * @brief set disturbance observer gain + * @param gain disturbance observer gain + */ + void setDisturbanceObserverGain(in double gain); + + /** + * @brief set the parcentage to the default servo gain + * @param name joint name, part name or "all" + * @param percentage to joint servo gain[0-100] + */ + void setServoTorqueGainPercentage(in string name, in double percentage); + + enum JointControlMode {FREE, POSITION, TORQUE, VELOCITY, POSITION_TORQUE}; + /** + * @brief set joint control mode + * @param jname joint name, part name or "all" + * @param jcm joint control mode + */ + void setJointControlMode(in string jname, in JointControlMode jcm); + }; }; diff --git a/idl/SequencePlayerService.idl b/idl/SequencePlayerService.idl index b6a67355e52..4cb3fd63d68 100644 --- a/idl/SequencePlayerService.idl +++ b/idl/SequencePlayerService.idl @@ -179,20 +179,20 @@ module OpenHRP boolean playPatternOfGroup(in string gname, in dSequenceSequence pos, in dSequence tm); /** - * @brief set parameters to solve ik, used in setTargetPose + * @brief set parameters to solve ik, used in setTargetPose [>= 315.1.5] * @param pos error threshold for position * @param rot error threshold for rotation */ void setMaxIKError(in double pos, in double rot); /** - * @brief set parameters to solve ik, used in setTargetPose + * @brief set parameters to solve ik, used in setTargetPose [>= 315.1.5] * @param max_iteration max iteration */ void setMaxIKIteration(in short max_iteration); /** - * @brief Interpolate Wrenches. Function returns without waiting for interpolation to finish + * @brief Interpolate Wrenches. Function returns without waiting for interpolation to finish [>= 315.2.0] * @param wrenches Wrenches [N], [Nm] * @param tm duration [s] * @return true if set successfully, false otherwise @@ -200,7 +200,7 @@ module OpenHRP boolean setWrenches(in dSequence wrenches, in double tm); /** - * @brief Interpolate all joint angles on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. + * @brief Interpolate all joint angles on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0] * @param jvs sequence of sequence of joint angles [rad] * @param tm sequence of duration [s] * @return true joint angles are set successfully, false otherwise @@ -208,7 +208,7 @@ module OpenHRP boolean setJointAnglesSequence(in dSequenceSequence jvss, in dSequence tms); /** - * @brief Interpolate all joint angles on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. + * @brief Interpolate all joint angles on robot using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0] * @param jvs sequence of sequence of joint angles [rad] * @param mask binary vector which selects joints to be interpolated * @param tm sequence of duration [s] @@ -217,7 +217,7 @@ module OpenHRP boolean setJointAnglesSequenceWithMask(in dSequenceSequence jvss, in bSequence mask, in dSequence tms); /** - * @brief Interpolate all joint angles in groups, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. + * @brief Interpolate all joint angles in groups, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0] * @param gname name of the joint group * @param jvs sequence of sequence of joint angles [rad] * @param tm sequence of duration [s] @@ -226,14 +226,14 @@ module OpenHRP boolean setJointAnglesSequenceOfGroup(in string gname, in dSequenceSequence jvss, in dSequence tms); /** - * @brief Interpolate all joint angles, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. + * @brief Interpolate all joint angles, using duration specified by \em tm. Returns without waiting for whole sequence to be sent to robot. If this function called during the robot moves, it overwrite current goal. [>= 315.5.0] * @param jvss sequence of sequence of joint angles [rad] * @param vels sequence of sequence of velocities [rad/s] - * @param accs sequence of sequence of torques [N] - * @param poss sequence of weist pos [mm] - * @param rpys sequence of weist rpy [rad] - * @param accs sequence of weist acc [m/ss] - * @param zmps sequence of weist zmp [mm] + * @param torques sequence of sequence of torques [Nm] + * @param poss sequence of waist pos [m] + * @param rpys sequence of waist rpy [rad] + * @param accs sequence of sensor acc [m/ss] + * @param zmps sequence of zmp in the waist frame [m] * @param wrenches sequence of wrenches [N, Nm] * @param optionss sequence of options [] * @param tms sequence of duration [s] @@ -242,12 +242,12 @@ module OpenHRP boolean setJointAnglesSequenceFull(in dSequenceSequence jvss, in dSequenceSequence vels, in dSequenceSequence torques, in dSequenceSequence poss, in dSequenceSequence rpys, in dSequenceSequence accs, in dSequenceSequence zmps, in dSequenceSequence wrenchs, in dSequenceSequence optionals, in dSequence tms); /** - * @brief clear current JointAngles + * @brief clear current JointAngles [>= 315.5.0] * @return true joint angles are set successfully, false otherwise */ boolean clearJointAngles(); /** - * @brief clear current JointAnglesOfGroup + * @brief clear current JointAnglesOfGroup [>= 315.5.0] * @param gname name of the joint group * @return true joint angles are set successfully, false otherwise */ diff --git a/idl/StabilizerService.idl b/idl/StabilizerService.idl index d9ed52d6aca..6a53f602a85 100644 --- a/idl/StabilizerService.idl +++ b/idl/StabilizerService.idl @@ -21,7 +21,8 @@ module OpenHRP TPCC, EEFM, EEFMQP, - EEFMQPCOP + EEFMQPCOP, + EEFMQPCOP2 }; /** @@ -43,7 +44,8 @@ module OpenHRP enum EmergencyCheckMode { NO_CHECK, COP, - CP + CP, + TILT }; /** @@ -77,6 +79,8 @@ module OpenHRP double reference_gain; /// Manipulability limit for inverse kinematics. double manipulability_limit; + /// IK loop count + unsigned short ik_loop_count; }; /** @@ -167,10 +171,22 @@ module OpenHRP double eefm_ee_rot_error_p_gain; /// Pos rot error cutoff freq [Hz] double eefm_ee_error_cutoff_freq; - /// Sequence of vertices for all end effectors + /// Sequence of vertices for all end effectors assuming that the order is (rleg, lleg, rarm, larm) sequence< SupportPolygonVertices > eefm_support_polygon_vertices_sequence; /// Use force difference control or each limb force control. True by default. boolean eefm_use_force_difference_control; + /// Use damping control for swing leg. + boolean eefm_use_swing_damping; + /// Swing damping control force threshold [N] + DblArray3 eefm_swing_damping_force_thre; + /// Swing damping control moment threshold [Nm] + DblArray3 eefm_swing_damping_moment_thre; + /// Rotation damping gain for swing leg [Nm/(rad/s)] (r,p,y) + DblArray3 eefm_swing_rot_damping_gain; + /// Position damping gain for swing leg [N/(m/s)] (x,y,z) + DblArray3 eefm_swing_pos_damping_gain; + /// Sequence of all end-effector force/moment distribution weight + sequence > eefm_ee_forcemoment_distribution_weight; // common /// Current Stabilizer algorithm STAlgorithm st_algorithm; @@ -188,10 +204,14 @@ module OpenHRP double cop_check_margin; /// CP margin [m] (front, rear, inside, outside) DblArray4 cp_check_margin; + /// tilt margin [rad] (single support phase, double support phase) from reference floor + DblArray2 tilt_margin; /// ref_CP [m] (x,y) (foot_origin relative coordinate) DblArray2 ref_capture_point; /// act_CP [m] (x,y) (foot_origin relative coordinate) DblArray2 act_capture_point; + /// CP_offset [m] (x,y) (foot_origin relative coordinate) + DblArray2 cp_offset; /// contact decision threshold [N] double contact_decision_threshold; /// Foot origin position offset @@ -203,6 +223,20 @@ module OpenHRP boolean is_estop_while_walking; /// Sequence for all end-effectors' ik limb parameters sequence ik_limb_parameters; + /// Whether change root link height for avoiding limb stretch + boolean use_limb_stretch_avoidance; + /// Limb stretch avoidance time constant [s] + double limb_stretch_avoidance_time_const; + /// Root link height change limitation for avoiding limb stretch [m/s] (lower, upper) + DblArray2 limb_stretch_avoidance_vlimit; + /// Sequence of limb length margin from max limb length [m] + sequence limb_length_margin; + /// Detection time whether is in air [s] + double detection_time_to_air; + /// Limit of compensation for difference between ref-act root rot [rad]. + DblArray2 root_rot_compensation_limit; + /// Whether truncate zmp according to foot support polygon or not + boolean use_zmp_truncation; }; /** diff --git a/launch/sample4legrobot.launch b/launch/sample4legrobot.launch index ffc7bba244f..04bdd629884 100644 --- a/launch/sample4legrobot.launch +++ b/launch/sample4legrobot.launch @@ -54,6 +54,8 @@ -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)" -o "example.CollisionDetector.config_file:$(arg CONF_FILE)" -o "example.SoftErrorLimiter.config_file:$(arg CONF_FILE)" + -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)" + -o "example.ObjectContactTurnaroundDetector.config_file:$(arg CONF_FILE)" -o "example.$(arg JOINT_CONTROLLER).config_file:$(arg CONF_FILE)" -o "corba.nameservers:localhost:$(arg corbaport)" $(arg hrpsys_gui_args)' diff --git a/launch/sample6dofrobot.launch b/launch/sample6dofrobot.launch index 87e117688e1..67dc43f9a70 100644 --- a/launch/sample6dofrobot.launch +++ b/launch/sample6dofrobot.launch @@ -35,6 +35,7 @@ -o "example.ImpedanceController.config_file:$(arg CONF_FILE)" -o "example.AutoBalancer.config_file:$(arg CONF_FILE)" -o "example.StateHolder.config_file:$(arg CONF_FILE)" + -o "example.DataLogger.config_file:$(arg CONF_FILE)" -o "example.TorqueFilter.config_file:$(arg CONF_FILE)" -o "example.TorqueController.config_file:$(arg CONF_FILE)" -o "example.ThermoEstimator.config_file:$(arg CONF_FILE)" @@ -44,8 +45,11 @@ -o "example.RemoveForceSensorLinkOffset.config_file:$(arg CONF_FILE)" -o "example.KalmanFilter.config_file:$(arg CONF_FILE)" -o "example.Stabilizer.config_file:$(arg CONF_FILE)" + -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)" -o "example.CollisionDetector.config_file:$(arg CONF_FILE)" -o "example.SoftErrorLimiter.config_file:$(arg CONF_FILE)" + -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)" + -o "example.ObjectContactTurnaroundDetector.config_file:$(arg CONF_FILE)" -o "corba.nameservers:localhost:$(arg corbaport)" $(arg hrpsys_gui_args)' output="screen" cwd="node" /> diff --git a/launch/samplerobot.launch b/launch/samplerobot.launch index 1c18fb6ac9e..83c03c5f52a 100644 --- a/launch/samplerobot.launch +++ b/launch/samplerobot.launch @@ -56,6 +56,8 @@ -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)" -o "example.CollisionDetector.config_file:$(arg CONF_FILE)" -o "example.SoftErrorLimiter.config_file:$(arg CONF_FILE)" + -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)" + -o "example.ObjectContactTurnaroundDetector.config_file:$(arg CONF_FILE)" -o "example.$(arg JOINT_CONTROLLER).config_file:$(arg CONF_FILE)" -o "corba.nameservers:localhost:$(arg corbaport)" $(arg hrpsys_gui_args)' diff --git a/launch/samplespecialjointrobot.launch b/launch/samplespecialjointrobot.launch index 7682995d8eb..bd22739addf 100644 --- a/launch/samplespecialjointrobot.launch +++ b/launch/samplespecialjointrobot.launch @@ -54,6 +54,8 @@ -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)" -o "example.CollisionDetector.config_file:$(arg CONF_FILE)" -o "example.SoftErrorLimiter.config_file:$(arg CONF_FILE)" + -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)" + -o "example.ObjectContactTurnaroundDetector.config_file:$(arg CONF_FILE)" -o "example.$(arg JOINT_CONTROLLER).config_file:$(arg CONF_FILE)" -o "corba.nameservers:localhost:$(arg corbaport)" $(arg hrpsys_gui_args)' diff --git a/lib/io/CMakeLists.txt b/lib/io/CMakeLists.txt index da13d427380..a83164fda94 100644 --- a/lib/io/CMakeLists.txt +++ b/lib/io/CMakeLists.txt @@ -12,8 +12,8 @@ option(INSTALL_HRPIO "Install dummy implementation of hrpIo" ON) if(INSTALL_HRPIO) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) endif() diff --git a/lib/io/iob.cpp b/lib/io/iob.cpp index 21f20510b0a..37147b4e4d0 100644 --- a/lib/io/iob.cpp +++ b/lib/io/iob.cpp @@ -27,8 +27,6 @@ static long g_period_ns=5000000; #define CHECK_ATTITUDE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_attitude_sensors()) return E_ID #if (defined __APPLE__) -typedef int clockid_t; -#define CLOCK_MONOTONIC 0 #include int clock_gettime(clockid_t clk_id, struct timespec *tp) { @@ -587,7 +585,65 @@ int number_of_thermometers() { return 0; } +#endif + +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 +int write_command_acceleration(int id, double acc) +{ + return FALSE; +} + +int write_command_accelerations(const double *accs) +{ + return FALSE; +} +int write_joint_inertia(int id, double mn) +{ + return FALSE; +} + +int write_joint_inertias(const double *mns) +{ + return FALSE; +} + +int read_pd_controller_torques(double *torques) +{ + return FALSE; +} + +int write_disturbance_observer(int com) +{ + return FALSE; +} + +int write_disturbance_observer_gain(double gain) +{ + return FALSE; +} +#endif + +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 +int read_torque_pgain(int id, double *gain) +{ + return FALSE; +} + +int write_torque_pgain(int id, double gain) +{ + return FALSE; +} + +int read_torque_dgain(int id, double *gain) +{ + return FALSE; +} + +int write_torque_dgain(int id, double gain) +{ + return FALSE; +} #endif int read_driver_temperature(int id, unsigned char *v) diff --git a/lib/io/iob.h b/lib/io/iob.h index fcaa3abc8af..0321beb7cab 100644 --- a/lib/io/iob.h +++ b/lib/io/iob.h @@ -63,6 +63,9 @@ extern "C"{ JCM_POSITION, ///< position control JCM_TORQUE, ///< torque control JCM_VELOCITY, ///< velocity control +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + JCM_POSITION_TORQUE, ///< position and torque control +#endif JCM_NUM } joint_control_mode; @@ -225,7 +228,7 @@ extern "C"{ * @retval FALSE otherwise */ int read_servo_alarm(int id, int *a); - + /** * @brief read joint control mode * @param id joint id @@ -578,6 +581,9 @@ extern "C"{ //@} #if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2 + /** + * @name IOB VERSION 2 + */ //@{ /** * @brief get the number of batteries @@ -604,6 +610,107 @@ extern "C"{ //@} #endif +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + /** + * @name IOB VERSION 3 + */ + //@{ + /** + * @brief write command angular acceleration[rad/s^2] + * @param id joint id + * @param acc angular acceleration [rad/s^2] + * @return TRUE or E_ID + */ + int write_command_acceleration(int id, double acc); + + /** + * @brief write command angular accelerations[rad/s^2] + * @param accs array of angular acceleration [rad/s^2] + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int write_command_accelerations(const double *accs); + + /** + * @brief write joint inertia + * @param id joint id + * @param mn joint inertia + * @return TRUE or E_ID + */ + int write_joint_inertia(int id, double mn); + + /** + * @brief write joint inertias + * @param mns array of joint inertia + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int write_joint_inertias(const double *mns); + + /** + * @brief read pd controller torques [Nm] + * @param torques array of pd controller torque [Nm] + * @retval TRUE this function is supported + * @retval FALSE otherwise + */ + int read_pd_controller_torques(double *torques); + + /** + * @brief turn on/off disturbance observer + * @param com ON/OFF + * @return TRUE if this function is supported, FALSE otherwise + */ + int write_disturbance_observer(int com); + + /** + * @brief write disturbance observer gain + * @param gain disturbance observer gain + * @return TRUE if this function is supported, FALSE otherwise + */ + int write_disturbance_observer_gain(double gain); + //@} +#endif + +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + /** + * @name IOB VERSION 4 + */ + //@{ + /** + * @brief read P gain[Nm/Nm] + * @param id joint id + * @param gain P gain[Nm/Nm] + * @return TRUE or E_ID + */ + int read_torque_pgain(int id, double *gain); + + /** + * @brief write P gain[Nm/Nm] + * @param id joint id + * @param gain P gain[Nm/Nm] + * @return TRUE or E_ID + */ + int write_torque_pgain(int id, double gain); + + /** + * @brief read D gain[Nm/(Nm/s)] + * @param id joint id + * @param gain D gain[Nm/(Nm/s)] + * @return TRUE or E_ID + */ + int read_torque_dgain(int id, double *gain); + + /** + * @brief write D gain[Nm/(Nm/s)] + * @param id joint id + * @param gain D gain[Nm/(Nm/s)] + * @return TRUE or E_ID + */ + int write_torque_dgain(int id, double gain); + + //@} +#endif + /** * @name thermometer */ diff --git a/lib/util/BVutil.cpp b/lib/util/BVutil.cpp index 55cb86643a9..48e9960d384 100644 --- a/lib/util/BVutil.cpp +++ b/lib/util/BVutil.cpp @@ -1,6 +1,10 @@ #include extern "C" { +#if (defined __APPLE__) +#include +#else #include +#endif } #include #include "BVutil.h" @@ -9,7 +13,7 @@ using namespace hrp; void convertToAABB(hrp::BodyPtr i_body) { - for (int i=0; inumLinks(); i++) convertToAABB(i_body->link(i)); + for (unsigned int i=0; inumLinks(); i++) convertToAABB(i_body->link(i)); } void convertToAABB(hrp::Link *i_link) @@ -74,7 +78,7 @@ void convertToAABB(hrp::Link *i_link) void convertToConvexHull(hrp::BodyPtr i_body) { - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ convertToConvexHull(i_body->link(i)); } } diff --git a/lib/util/BodyRTC.cpp b/lib/util/BodyRTC.cpp index ad8e97fe7d4..b751acc73e3 100644 --- a/lib/util/BodyRTC.cpp +++ b/lib/util/BodyRTC.cpp @@ -73,12 +73,13 @@ RTC::ReturnCode_t BodyRTC::setup(){ servo_status.resize(numJoints()); power_status.resize(numJoints()); m_servoErrorLimit.resize(numJoints()); - for(int i = 0; i < numJoints(); i++) { + for(unsigned int i = 0; i < numJoints(); i++) { calib_status[i] = servo_status[i] = power_status[i] = OpenHRP::RobotHardwareService::SWITCH_ON; m_servoErrorLimit[i] = DEFAULT_ANGLE_ERROR_LIMIT; } m_emergencyReason = EMG_NONE; // clear m_emergencyId = -1; + return RTC::RTC_OK; } void parsePortConfig(const std::string &config, @@ -118,7 +119,7 @@ bool getJointList(hrp::Body *body, const std::vector &elements, std::vector &joints) { if (elements.size() == 0){ - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ joints.push_back(body->joint(i)); } }else{ @@ -462,7 +463,7 @@ bool BodyRTC::setServoErrorLimit(const char *i_jname, double i_limit) { Link *l = NULL; if (strcmp(i_jname, "all") == 0 || strcmp(i_jname, "ALL") == 0){ - for (int i=0; icalcSubMassCM(); bool all_servo_off = true; bool emulate_highgain_servo_off_mode = (numJoints() > 0); // If no joints, do not use servo off emulation - for(int i = 0; i < numJoints(); ++i){ + for(unsigned int i = 0; i < numJoints(); ++i){ Link *j = joint(i); commands[i] = j->q; int p = readPowerState(i); @@ -562,26 +563,27 @@ bool BodyRTC::preOneStep() { m_lastServoOn_R = rootLink()->attitude(); } } + return true; } bool BodyRTC::postOneStep() { - for(int i = 0; i < numJoints(); ++i){ + for(unsigned int i = 0; i < numJoints(); ++i){ angles[i] = joint(i)->q; } - for(int i = 0; i < numSensors(hrp::Sensor::ACCELERATION); i++ ){ + for(unsigned int i = 0; i < numSensors(hrp::Sensor::ACCELERATION); i++ ){ hrp::AccelSensor *s = sensor(i); accels[i][0] = s->dv[0]; accels[i][1] = s->dv[1]; accels[i][2] = s->dv[2]; } - for(int i = 0; i < numSensors(hrp::Sensor::RATE_GYRO); i++ ){ + for(unsigned int i = 0; i < numSensors(hrp::Sensor::RATE_GYRO); i++ ){ hrp::RateGyroSensor *s = sensor(i); gyros[i][0] = s->w[0]; gyros[i][1] = s->w[1]; gyros[i][2] = s->w[2]; } - for(int i = 0; i < numSensors(hrp::Sensor::FORCE); i++ ){ + for(unsigned int i = 0; i < numSensors(hrp::Sensor::FORCE); i++ ){ hrp::ForceSensor *s = sensor(i); forces[i][0] = s->f[0]; forces[i][1] = s->f[1]; @@ -605,7 +607,7 @@ bool BodyRTC::servo(const char *jname, bool turnon) Link *l = NULL; if (strcmp(jname, "all") == 0 || strcmp(jname, "ALL") == 0){ bool ret = true; - for (int i=0; ipower(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON); + return m_robot->power(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON); } CORBA::Boolean RobotHardwareServicePort::servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus turnon) { - m_robot->servo(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON); + return m_robot->servo(jname, turnon == OpenHRP::RobotHardwareService::SWITCH_ON); } void RobotHardwareServicePort::setServoGainPercentage(const char *jname, double limit) { } +void RobotHardwareServicePort::setServoPGainPercentage(const char *jname, double limit) { +} +void RobotHardwareServicePort::setServoDGainPercentage(const char *jname, double limit) { +} +void RobotHardwareServicePort::setServoPGainPercentageWithTime(const char *jname, double limit, double time) { +} +void RobotHardwareServicePort::setServoDGainPercentageWithTime(const char *jname, double limit, double time) { +} +void RobotHardwareServicePort::setServoTorqueGainPercentage(const char *jname, double limit) { +} void RobotHardwareServicePort::setServoErrorLimit(const char *jname, double limit) { m_robot->setServoErrorLimit(jname, limit); } +void RobotHardwareServicePort::setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm){ +} void RobotHardwareServicePort::calibrateInertiaSensor() { } void RobotHardwareServicePort::removeForceSensorOffset() { @@ -702,6 +716,18 @@ CORBA::Long RobotHardwareServicePort::lengthDigitalOutput() { CORBA::Boolean RobotHardwareServicePort::readDigitalOutput(::OpenHRP::RobotHardwareService::OctSequence_out dout) { return false; } + +CORBA::Boolean RobotHardwareServicePort::setJointInertia(const char* name, ::CORBA::Double mn) +{ + return true; +} + +void RobotHardwareServicePort::setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence& mns) +{ +} +void RobotHardwareServicePort::enableDisturbanceObserver(){} +void RobotHardwareServicePort::disableDisturbanceObserver(){} +void RobotHardwareServicePort::setDisturbanceObserverGain(::CORBA::Double gain){} // void RobotHardwareServicePort::setRobot(BodyRTC *i_robot) { m_robot = i_robot; } diff --git a/lib/util/BodyRTC.h b/lib/util/BodyRTC.h index dbd9e30ad48..a0bf4a72fb5 100644 --- a/lib/util/BodyRTC.h +++ b/lib/util/BodyRTC.h @@ -1,6 +1,10 @@ #ifndef BODY_EXT_H_INCLUDED #define BODY_EXT_H_INCLUDED +#include +#include +#include "hrpsys/idl/Img.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include #include #include @@ -10,8 +14,6 @@ #include #include #include -#include "Img.hh" -#include "RobotHardwareService.hh" class InPortHandlerBase; class OutPortHandlerBase; @@ -30,7 +32,13 @@ class RobotHardwareServicePort CORBA::Boolean power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss); CORBA::Boolean servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss); void setServoGainPercentage(const char *jname, double limit); + void setServoPGainPercentage(const char *jname, double limit); + void setServoDGainPercentage(const char *jname, double limit); + void setServoPGainPercentageWithTime(const char *jname, double limit, double time); + void setServoDGainPercentageWithTime(const char *jname, double limit, double time); + void setServoTorqueGainPercentage(const char *jname, double limit); void setServoErrorLimit(const char *jname, double limit); + void setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm); void calibrateInertiaSensor(); void removeForceSensorOffset(); void initializeJointAngle(const char* name, const char* option); @@ -41,6 +49,11 @@ class RobotHardwareServicePort CORBA::Boolean writeDigitalOutputWithMask(const ::OpenHRP::RobotHardwareService::OctSequence& dout, const ::OpenHRP::RobotHardwareService::OctSequence& mask); CORBA::Long lengthDigitalOutput(); CORBA::Boolean readDigitalOutput(::OpenHRP::RobotHardwareService::OctSequence_out dout); + CORBA::Boolean setJointInertia(const char* name, ::CORBA::Double mn); + void setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence& mns); + void enableDisturbanceObserver(); + void disableDisturbanceObserver(); + void setDisturbanceObserverGain(::CORBA::Double gain); void setRobot(BodyRTC *i_robot); private: BodyRTC *m_robot; @@ -78,12 +91,12 @@ class BodyRTC : virtual public hrp::Body, public RTC::DataFlowComponentBase bool postOneStep(); bool names2ids(const std::vector &i_names, std::vector &o_ids); std::vector getJointGroup(const char* gname) {return m_jointGroups[gname]; } - int addJointGroup(const char* gname, const std::vectorjids) { m_jointGroups[gname] = jids; } + int addJointGroup(const char* gname, const std::vectorjids) { m_jointGroups[gname] = jids; return 0;} // API to be compatible with robot.h - bool power(int jid, bool turnon) {power_status[jid] = turnon?OpenHRP::RobotHardwareService::SWITCH_ON:OpenHRP::RobotHardwareService::SWITCH_OFF; } + bool power(int jid, bool turnon) {power_status[jid] = turnon?OpenHRP::RobotHardwareService::SWITCH_ON:OpenHRP::RobotHardwareService::SWITCH_OFF; return true; } bool servo(const char *jname, bool turnon); - bool servo(int jid, bool turnon) {servo_status[jid] = turnon?OpenHRP::RobotHardwareService::SWITCH_ON:OpenHRP::RobotHardwareService::SWITCH_OFF; } + bool servo(int jid, bool turnon) {servo_status[jid] = turnon?OpenHRP::RobotHardwareService::SWITCH_ON:OpenHRP::RobotHardwareService::SWITCH_OFF; return true; } bool power(const char *jname, bool turnon); //void removeForceSensorOffset(); @@ -127,7 +140,7 @@ class BodyRTC : virtual public hrp::Body, public RTC::DataFlowComponentBase int lengthDigitalOutput(); bool readDigitalOutput(char *o_dout); - bool resetPosition() { m_resetPosition = true; } + bool resetPosition() { m_resetPosition = true; return true; } // BodyRTC::emg_reason m_emergencyReason; int m_emergencyId; diff --git a/lib/util/CMakeLists.txt b/lib/util/CMakeLists.txt index f01b8e8c9d9..fd4a66ed49b 100644 --- a/lib/util/CMakeLists.txt +++ b/lib/util/CMakeLists.txt @@ -1,3 +1,9 @@ +if (APPLE) + include_directories(${PCL_INCLUDE_DIRS}) + link_directories(${PCL_LIBRARY_DIRS}) + add_definitions(${PCL_DEFINITIONS}) +endif() + set(sources Project.cpp ProjectUtil.cpp @@ -57,15 +63,15 @@ target_link_libraries(hrpsysUtil ${GLUT_LIBRARIES} ${QHULL_LIBRARIES} GLEW - boost_thread + ${Boost_THREAD_LIBRARY} boost_system ) set(target hrpsysUtil) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) install(FILES ${headers} DESTINATION include/hrpsys/util) diff --git a/lib/util/GLbody.cpp b/lib/util/GLbody.cpp index f15ba2a4fff..e348fb5f7ab 100644 --- a/lib/util/GLbody.cpp +++ b/lib/util/GLbody.cpp @@ -93,7 +93,7 @@ boost::function2 GLbody::getSensorDrawCallback void GLbody::divideLargeTriangles(double maxEdgeLen) { - for (int i=0; idivideLargeTriangles(maxEdgeLen); } diff --git a/lib/util/GLbodyRTC.cpp b/lib/util/GLbodyRTC.cpp index 26f4c778645..b9fdf6b7e66 100644 --- a/lib/util/GLbodyRTC.cpp +++ b/lib/util/GLbodyRTC.cpp @@ -1,4 +1,4 @@ -#include "util/GLbodyRTC.h" +#include "hrpsys/util/GLbodyRTC.h" const char* GLbodyRTC::glbodyrtc_spec[] = { diff --git a/lib/util/GLbodyRTC.h b/lib/util/GLbodyRTC.h index 32c9eb8cd2a..bc0e1c8bd38 100644 --- a/lib/util/GLbodyRTC.h +++ b/lib/util/GLbodyRTC.h @@ -1,8 +1,8 @@ #ifndef __GLBODYRTC_H__ #define __GLBODYRTC_H__ -#include "util/BodyRTC.h" -#include "util/GLbody.h" +#include "hrpsys/util/BodyRTC.h" +#include "hrpsys/util/GLbody.h" class GLbodyRTC : public BodyRTC, public GLbody { diff --git a/lib/util/GLcamera.cpp b/lib/util/GLcamera.cpp index b0e11fd3c05..6d8a4bf90e5 100644 --- a/lib/util/GLcamera.cpp +++ b/lib/util/GLcamera.cpp @@ -21,9 +21,9 @@ using namespace hrp; GLcamera::GLcamera(int i_width, int i_height, double i_near, double i_far, double i_fovy, GLlink *i_link, int i_id) : + m_link(i_link), m_near(i_near), m_far(i_far), m_fovy(i_fovy), m_width(i_width), m_height(i_height), - m_link(i_link), m_frameBuffer(0), m_renderBuffer(0), m_texture(0), m_sensor(NULL), m_colorBuffer(NULL) { @@ -160,7 +160,7 @@ void GLcamera::render(GLsceneBase *i_scene) }else{ unsigned char *src=m_colorBuffer; unsigned char *dst=&m_sensor->image[m_width*(m_height-1)*3]; - for (int i=0; iimage[m_width*(m_height-1)]; - for (int i=0; i m_shapes; GLuint m_frameBuffer, m_renderBuffer, m_texture; diff --git a/lib/util/GLsceneBase.cpp b/lib/util/GLsceneBase.cpp index bb609303372..f132cc1a587 100644 --- a/lib/util/GLsceneBase.cpp +++ b/lib/util/GLsceneBase.cpp @@ -26,9 +26,9 @@ static void drawString(const char *str) } GLsceneBase::GLsceneBase(LogManagerBase *i_log) : - m_width(DEFAULT_W), m_height(DEFAULT_H), m_showingStatus(false), m_showSlider(false), - m_log(i_log), m_videoWriter(NULL), m_cvImage(NULL), + m_width(DEFAULT_W), m_height(DEFAULT_H), + m_videoWriter(NULL), m_cvImage(NULL), m_log(i_log), m_showFloorGrid(true), m_showInfo(true), m_defaultLights(true), m_request(REQ_NONE), m_maxEdgeLen(0), @@ -67,9 +67,9 @@ void GLsceneBase::setCamera(GLcamera *i_camera) void GLsceneBase::nextCamera() { bool found = m_camera == m_default_camera ? true : false; - for (int i=0; inumLinks(); j++){ + for (unsigned int j=0; jnumLinks(); j++){ GLlink *l = dynamic_cast(b->link(j)); const std::vector& cameras = l->cameras(); for (size_t k=0; k=0 && m_targetObject < numBodies()){ + if (m_targetObject >=0 && m_targetObject < (int)numBodies()){ sprintf(buf, "Target: %s", targetObject()->name().c_str()); h -= 15; glRasterPos2f(10, h); @@ -272,7 +272,7 @@ size_t GLsceneBase::drawObjects(bool showSensors) { size_t ntri = 0; boost::function2 callback; - for (int i=0; i(body(i).get()); if (!glbody) std::cout << "dynamic_cast failed" << std::endl; if (!showSensors) { @@ -369,9 +369,9 @@ void GLsceneBase::draw() } // offscreen redering - for (int i=0; inumLinks(); j++){ + for (unsigned int j=0; jnumLinks(); j++){ GLlink *l = dynamic_cast(b->link(j)); const std::vector& cameras = l->cameras(); for (size_t k=0; k= 0 && m_targetObject < numBodies()){ + if (m_targetObject >= 0 && m_targetObject < (int)numBodies()){ return body(m_targetObject); }else{ return hrp::BodyPtr(); @@ -442,7 +442,7 @@ void GLsceneBase::setBackGroundColor(float rgb[3]) hrp::Vector3 GLsceneBase::center() { hrp::Vector3 mi,ma,min,max; - for (int i=0; i(body(i).get()); glbody->computeAABB(mi,ma); if (i==0){ diff --git a/lib/util/GLshape.cpp b/lib/util/GLshape.cpp index 91a17eba286..966fbdd883b 100644 --- a/lib/util/GLshape.cpp +++ b/lib/util/GLshape.cpp @@ -10,7 +10,7 @@ #include "GLshape.h" #include "GLtexture.h" -GLshape::GLshape() : m_texture(NULL), m_requestCompile(false), m_shininess(0.2), m_shadingList(0), m_wireFrameList(0), m_highlight(false) +GLshape::GLshape() : m_shininess(0.2), m_texture(NULL), m_requestCompile(false), m_shadingList(0), m_wireFrameList(0), m_highlight(false) { for (int i=0; i<16; i++) m_trans[i] = 0.0; m_trans[0] = m_trans[5] = m_trans[10] = m_trans[15] = 1.0; @@ -41,7 +41,7 @@ size_t GLshape::draw(int i_mode) return m_triangles.size(); } -void GLshape::setVertices(int nvertices, const float *vertices) +void GLshape::setVertices(unsigned int nvertices, const float *vertices) { m_vertices.resize(nvertices); for (size_t i=0; isetColors(ai.colors.length()/3, ai.colors.get_buffer()); if (ai.textureIndex >=0){ - if (txs->length() <= ai.textureIndex){ + if (txs->length() <= (unsigned int)ai.textureIndex){ std::cerr << "invalid texture index(" << ai.textureIndex << ")" << std::endl; }else{ diff --git a/lib/util/Hrpsys.h b/lib/util/Hrpsys.h index af98c12a1fa..65758d40438 100644 --- a/lib/util/Hrpsys.h +++ b/lib/util/Hrpsys.h @@ -12,6 +12,8 @@ #include #include #include +#include +#include #include using std::FILE; using std::exp; diff --git a/lib/util/OpenRTMUtil.cpp b/lib/util/OpenRTMUtil.cpp index a85eb1921c6..bb8bf6056ff 100644 --- a/lib/util/OpenRTMUtil.cpp +++ b/lib/util/OpenRTMUtil.cpp @@ -85,7 +85,6 @@ const char *getServiceIOR(RTC::RTObject_var rtc, connProfile.connector_id = ""; connProfile.ports.length(1); connProfile.ports[0] = port; - connProfile.properties = NULL; port->connect(connProfile); connProfile.properties[0].value >>= ior; diff --git a/lib/util/PortHandler.cpp b/lib/util/PortHandler.cpp index 50256e159c7..d454d839867 100644 --- a/lib/util/PortHandler.cpp +++ b/lib/util/PortHandler.cpp @@ -272,6 +272,16 @@ VisionSensorPortHandler::VisionSensorPortHandler( m_data.data.image.format = Img::CF_RGB; int len = m_sensor->width*m_sensor->height*3; m_data.data.image.raw_data.length(len); + m_data.data.intrinsic.distortion_coefficient.length(5); + for(int i = 0; i < 5; i++){ + m_data.data.intrinsic.distortion_coefficient[i] = 0; + } + double fovx = m_sensor->width/m_sensor->height*m_sensor->fovy; + m_data.data.intrinsic.matrix_element[0]=0.5 * m_sensor->width/tan(fovx/2.0); + m_data.data.intrinsic.matrix_element[1]=0.0; + m_data.data.intrinsic.matrix_element[2]=m_sensor->width/2; + m_data.data.intrinsic.matrix_element[3]=0.5 * m_sensor->height/tan(m_sensor->fovy/2.0); + m_data.data.intrinsic.matrix_element[4]=m_sensor->height/2; }else if(m_sensor->imageType == VisionSensor::MONO || m_sensor->imageType == VisionSensor::MONO_DEPTH){ m_data.data.image.width = m_sensor->width; @@ -279,6 +289,16 @@ VisionSensorPortHandler::VisionSensorPortHandler( m_data.data.image.format = Img::CF_GRAY; int len = m_sensor->width*m_sensor->height; m_data.data.image.raw_data.length(len); + m_data.data.intrinsic.distortion_coefficient.length(5); + for(int i = 0; i < 5; i++){ + m_data.data.intrinsic.distortion_coefficient[i] = 0; + } + double fovx = m_sensor->width/m_sensor->height*m_sensor->fovy; + m_data.data.intrinsic.matrix_element[0]=0.5 * m_sensor->width/tan(fovx/2.0); + m_data.data.intrinsic.matrix_element[1]=0.0; + m_data.data.intrinsic.matrix_element[2]=m_sensor->width/2; + m_data.data.intrinsic.matrix_element[3]=0.5 * m_sensor->height/tan(m_sensor->fovy/2.0); + m_data.data.intrinsic.matrix_element[4]=m_sensor->height/2; } } diff --git a/lib/util/PortHandler.h b/lib/util/PortHandler.h index dffd5ed5f60..2f40b6dcbbb 100644 --- a/lib/util/PortHandler.h +++ b/lib/util/PortHandler.h @@ -2,9 +2,9 @@ #define __PORT_HANDLER_H__ #include -#include "HRPDataTypes.hh" +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/pointcloud.hh" #include "BodyRTC.h" -#include "pointcloud.hh" namespace hrp{ class ForceSensor; diff --git a/lib/util/ProjectUtil.h b/lib/util/ProjectUtil.h index 1a4db29a954..5dc5158fa35 100644 --- a/lib/util/ProjectUtil.h +++ b/lib/util/ProjectUtil.h @@ -3,8 +3,8 @@ #include #include #include -#include "util/Project.h" -#include "util/OpenRTMUtil.h" +#include "hrpsys/util/Project.h" +#include "hrpsys/util/OpenRTMUtil.h" typedef boost::function2 BodyFactory; diff --git a/lib/util/SDLUtil.cpp b/lib/util/SDLUtil.cpp index c0ce1e5ffd9..4424b33d0f9 100644 --- a/lib/util/SDLUtil.cpp +++ b/lib/util/SDLUtil.cpp @@ -7,11 +7,11 @@ #include #endif #include -#include "util/ThreadedObject.h" -#include "util/LogManagerBase.h" -#include "util/GLsceneBase.h" -#include "util/GLcamera.h" -#include "util/GLlink.h" +#include "hrpsys/util/ThreadedObject.h" +#include "hrpsys/util/LogManagerBase.h" +#include "hrpsys/util/GLsceneBase.h" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" #include "SDLUtil.h" diff --git a/lib/util/ThreadedObject.cpp b/lib/util/ThreadedObject.cpp index d5273b4ef2e..1720cc9c3fa 100644 --- a/lib/util/ThreadedObject.cpp +++ b/lib/util/ThreadedObject.cpp @@ -10,7 +10,7 @@ static int threadMain(void *arg) } ThreadedObject::ThreadedObject() : - m_thread(NULL), m_isPausing(false), m_isRunning(false) + m_isPausing(false), m_isRunning(false), m_thread(NULL) { m_sem = SDL_CreateSemaphore(0); } diff --git a/lib/util/VectorConvert.h b/lib/util/VectorConvert.h index 31c0b27703d..ebf5ca747c9 100644 --- a/lib/util/VectorConvert.h +++ b/lib/util/VectorConvert.h @@ -4,15 +4,24 @@ #include #include #include -#include #include +// Clang (and apparently ISO C++) doesn't like it when functions are not +// declared before use, *even when the use occurs in templates*. + +template +std::istream& operator>>(std::istream& is, std::vector& v); +std::istream& operator>>(std::istream& is, hrp::dvector& v); +std::istream& operator>>(std::istream& is, hrp::Vector3& v); + +#include + template std::istream& operator>>(std::istream& is, std::vector& v) { std::string s; std::vector sv; - is >> s; + getline(is,s); sv = coil::split(s ,","); v.resize(sv.size()); for (int i(0), len(sv.size()); i < len; ++i) @@ -30,7 +39,7 @@ std::istream& operator>>(std::istream& is, hrp::dvector& v) { std::string s; std::vector sv; - is >> s; + getline(is,s); sv = coil::split(s ,","); v.resize(sv.size()); for (int i(0), len(sv.size()); i < len; ++i) @@ -48,7 +57,7 @@ std::istream& operator>>(std::istream& is, hrp::Vector3& v) { std::string s; std::vector sv; - is >> s; + getline(is,s); sv = coil::split(s ,","); for (int i(0); i < 3; ++i) { diff --git a/package.xml b/package.xml index 1ab45e069cd..cf51b3b8823 100644 --- a/package.xml +++ b/package.xml @@ -1,6 +1,6 @@ hrpsys - 315.8.0 + 315.15.0

    An OpenRTM-aist-based robot controller. This package is the most tailored for humanoid (dual-arm and/or biped) robots for historical reason.

    hrpsys package does not only wraps but build and installs the code from its mainstream repository (github.com/fkanehiro/hrpsys-base).

    @@ -17,7 +17,6 @@ EPL Kei Okada - Isaac Isao Saito AIST Fumio Kanehiro @@ -42,6 +41,7 @@ openhrp3 python-tk sdl + proj cv_bridge glut diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index 8718248fc72..09673658fe1 100755 --- a/python/hrpsys_config.py +++ b/python/hrpsys_config.py @@ -12,6 +12,7 @@ import socket import time import subprocess +from distutils.version import StrictVersion # copy from transformations.py, Christoph Gohlke, The Regents of the University of California @@ -157,7 +158,7 @@ def euler_from_matrix(matrix, axes='sxyz'): # class for configure hrpsys RTCs and ports # In order to specify robot-dependent code, please inherit this HrpsysConfigurator -class HrpsysConfigurator: +class HrpsysConfigurator(object): # RobotHardware rh = None @@ -247,6 +248,26 @@ class HrpsysConfigurator: log_version = None log_use_owned_ec = False + # Beeper + bp = None + bp_svc = None + bp_version = None + + # ReferenceForceUpdater + rfu = None + rfu_svc = None + rfu_version = None + + # Acceleration Filter + acf = None + acf_svc = None + acf_version = None + + # ObjectContactTurnaroundDetector + octd = None + octd_svc = None + octd_version = None + # rtm manager ms = None @@ -345,27 +366,54 @@ def connectComps(self): connectPorts(self.seq.port("basePos"), self.sh.port("basePosIn")) connectPorts(self.seq.port("baseRpy"), self.sh.port("baseRpyIn")) connectPorts(self.seq.port("zmpRef"), self.sh.port("zmpIn")) - if self.seq_version >= '315.2.6': + if StrictVersion(self.seq_version) >= StrictVersion('315.2.6'): connectPorts(self.seq.port("optionalData"), self.sh.port("optionalDataIn")) connectPorts(self.sh.port("basePosOut"), [self.seq.port("basePosInit"), self.fk.port("basePosRef")]) connectPorts(self.sh.port("baseRpyOut"), [self.seq.port("baseRpyInit"), self.fk.port("baseRpyRef")]) connectPorts(self.sh.port("qOut"), self.seq.port("qInit")) - if self.seq_version >= '315.2.0': + if StrictVersion(self.seq_version) >= StrictVersion('315.2.0'): connectPorts(self.sh.port("zmpOut"), self.seq.port("zmpRefInit")) for sen in self.getForceSensorNames(): connectPorts(self.seq.port(sen + "Ref"), self.sh.port(sen + "In")) + if self.es: + connectPorts(self.seq.port("qEmergency"), self.es.port("qEmergency")) - # connection for st + # connection for abc if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort( - self.rh.ref, "rfsensor") and self.st: - connectPorts(self.kf.port("rpy"), self.st.port("rpy")) + self.rh.ref, "rfsensor") and self.abc: + connectPorts(self.kf.port("rpy"), self.abc.port("rpy")) connectPorts(self.sh.port("zmpOut"), self.abc.port("zmpIn")) connectPorts(self.sh.port("basePosOut"), self.abc.port("basePosIn")) connectPorts(self.sh.port("baseRpyOut"), self.abc.port("baseRpyIn")) connectPorts(self.sh.port("optionalDataOut"), self.abc.port("optionalData")) + connectPorts(self.abc.port("accRef"), self.kf.port("accRef")) + connectPorts(self.rh.port("q"), self.abc.port("qCurrent")) + connectPorts(self.seq.port("qRef"), self.abc.port("qRefSeq")) + if self.es: + connectPorts(self.abc.port("emergencySignal"), self.es.port("emergencySignal")) + connectPorts(self.abc.port("emergencyFallMotion"), self.es.port("emergencyFallMotion")) + connectPorts(self.es.port("touchWallMotionSolved"), self.abc.port("touchWallMotionSolved")) + connectPorts(self.es.port("qTouchWall"), self.abc.port("qTouchWall")) + if self.rfu: + connectPorts(self.abc.port("diffFootOriginExtMoment"), self.rfu.port("diffFootOriginExtMoment")) + connectPorts(self.rfu.port("refFootOriginExtMoment"), self.abc.port("refFootOriginExtMoment")) + connectPorts(self.rfu.port("refFootOriginExtMomentIsHoldValue"), self.abc.port("refFootOriginExtMomentIsHoldValue")) + if self.octd: + connectPorts(self.abc.port("contactStates"), self.octd.port("contactStates")) + connectPorts(self.abc.port("tau"), self.rh.port("tauRef")) + connectPorts(self.abc.port("RobotHardwareService"), self.rh.port("RobotHardwareService")) + + # connection for st + if rtm.findPort(self.rh.ref, "lfsensor") and rtm.findPort( + self.rh.ref, "rfsensor") and self.st: + connectPorts(self.kf.port("rpy"), self.st.port("rpy")) + connectPorts(self.sh.port("zmpOut"), self.st.port("zmpIn")) + connectPorts(self.sh.port("basePosOut"), self.st.port("basePosIn")) + connectPorts(self.sh.port("baseRpyOut"), self.st.port("baseRpyIn")) + connectPorts(self.sh.port("optionalDataOut"), self.st.port("optionalData")) connectPorts(self.abc.port("zmpOut"), self.st.port("zmpRef")) connectPorts(self.abc.port("baseRpyOut"), self.st.port("baseRpyIn")) connectPorts(self.abc.port("basePosOut"), self.st.port("basePosIn")) @@ -376,34 +424,47 @@ def connectComps(self): connectPorts(self.seq.port("qRef"), self.st.port("qRefSeq")) connectPorts(self.abc.port("walkingStates"), self.st.port("walkingStates")) connectPorts(self.abc.port("sbpCogOffset"), self.st.port("sbpCogOffset")) - if self.es: - connectPorts(self.st.port("emergencySignal"), self.es.port("emergencySignal")) + + connectPorts(self.abc.port("toeheelRatio"), self.st.port("toeheelRatio")) connectPorts(self.st.port("emergencySignal"), self.abc.port("emergencySignal")) + connectPorts(self.st.port("diffCapturePoint"), self.abc.port("diffCapturePoint")) + connectPorts(self.st.port("actContactStates"), self.abc.port("actContactStates")) + if self.rfu: + connectPorts(self.st.port("diffFootOriginExtMoment"), self.rfu.port("diffFootOriginExtMoment")) + connectPorts(self.rfu.port("refFootOriginExtMoment"), self.abc.port("refFootOriginExtMoment")) + connectPorts(self.rfu.port("refFootOriginExtMomentIsHoldValue"), self.abc.port("refFootOriginExtMomentIsHoldValue")) + if self.octd: + connectPorts(self.abc.port("contactStates"), self.octd.port("contactStates")) # ref force moment connection for sen in self.getForceSensorNames(): - if self.st: + if self.abc and self.st: connectPorts(self.abc.port(sen), self.st.port(sen + "Ref")) + connectPorts(self.abc.port("limbCOPOffset_"+sen), + self.st.port("limbCOPOffset_"+sen)) if self.es: - connectPorts(self.sh.port(sen+"Out"), - self.es.port(sen+"In")) - if self.ic: - connectPorts(self.es.port(sen+"Out"), - self.ic.port("ref_" + sen+"In")) - if self.abc: - connectPorts(self.es.port(sen+"Out"), - self.abc.port("ref_" + sen)) + ref_force_port_from = self.es.port(sen+"Out") + elif self.rfu: + ref_force_port_from = self.rfu.port("ref_"+sen+"Out") else: - if self.ic: + ref_force_port_from = self.sh.port(sen+"Out") + if self.ic: + connectPorts(ref_force_port_from, + self.ic.port("ref_" + sen+"In")) + if self.abc: + connectPorts(ref_force_port_from, + self.abc.port("ref_" + sen)) + if self.rfu: connectPorts(self.sh.port(sen+"Out"), - self.ic.port("ref_" + sen+"In")) - if self.abc: + self.rfu.port("ref_" + sen+"In")) + if self.es: + connectPorts(self.rfu.port("ref_" + sen+"Out"), + self.es.port(sen+"In")) + else: + if self.es: connectPorts(self.sh.port(sen+"Out"), - self.abc.port("ref_" + sen)) - if self.abc and self.st: - connectPorts(self.abc.port("limbCOPOffset_"+sen), - self.st.port("limbCOPOffset_"+sen)) + self.es.port(sen+"In")) # actual force sensors if self.rmfo: @@ -416,9 +477,15 @@ def connectComps(self): if self.ic: connectPorts(self.rmfo.port("off_" + sen.name), self.ic.port(sen.name)) + if self.rfu: + connectPorts(self.rmfo.port("off_" + sen.name), + self.rfu.port(sen.name)) if self.st: connectPorts(self.rmfo.port("off_" + sen.name), self.st.port(sen.name)) + if self.abc: + connectPorts(self.rmfo.port("off_" + sen.name), + self.abc.port(sen.name)) elif self.ic: # if the robot does not have rmfo and kf, but have ic for sen in filter(lambda x: x.type == "Force", self.sensors): connectPorts(self.rh.port(sen.name), @@ -427,9 +494,15 @@ def connectComps(self): # connection for ic if self.ic: connectPorts(self.rh.port("q"), self.ic.port("qCurrent")) - if self.seq_version >= '315.3.0': + if StrictVersion(self.seq_version) >= StrictVersion('315.3.0'): connectPorts(self.sh.port("basePosOut"), self.ic.port("basePosIn")) connectPorts(self.sh.port("baseRpyOut"), self.ic.port("baseRpyIn")) + # connection for rfu + if self.rfu: + connectPorts(self.sh.port("qOut"), self.rfu.port("qRef")) + if StrictVersion(self.seq_version) >= StrictVersion('315.3.0'): + connectPorts(self.sh.port("basePosOut"), self.rfu.port("basePosIn")) + connectPorts(self.sh.port("baseRpyOut"), self.rfu.port("baseRpyIn")) # connection for tf if self.tf: # connection for actual torques @@ -449,8 +522,14 @@ def connectComps(self): if self.co: connectPorts(self.rh.port("q"), self.co.port("qCurrent")) connectPorts(self.rh.port("servoState"), self.co.port("servoStateIn")) - - + # connection for octd + if self.octd: + connectPorts(self.rh.port("q"), self.octd.port("qCurrent")) + if self.kf: + connectPorts(self.kf.port("rpy"), self.octd.port("rpy")) + if self.rmfo: + for sen in filter(lambda x: x.type == "Force", self.sensors): + connectPorts(self.rmfo.port("off_" + sen.name), self.octd.port(sen.name)) # connection for gc if self.gc: connectPorts(self.rh.port("q"), self.gc.port("qCurrent")) # other connections @@ -492,10 +571,35 @@ def connectComps(self): if self.el: connectPorts(self.rh.port("q"), self.el.port("qCurrent")) - # connection for co + # connection for es if self.es: connectPorts(self.rh.port("servoState"), self.es.port("servoStateIn")) + # connection for bp + if self.bp: + if self.tl: + connectPorts(self.tl.port("beepCommand"), self.bp.port("beepCommand")) + if self.es: + connectPorts(self.es.port("beepCommand"), self.bp.port("beepCommand")) + if self.el: + connectPorts(self.el.port("beepCommand"), self.bp.port("beepCommand")) + if self.co: + connectPorts(self.co.port("beepCommand"), self.bp.port("beepCommand")) + + # connection for acf + if self.acf: + # currently use first acc and rate sensors for acf + s_acc = filter(lambda s: s.type == 'Acceleration', self.sensors) + if (len(s_acc) > 0) and self.rh.port(s_acc[0].name) != None: + connectPorts(self.rh.port(s_acc[0].name), self.acf.port('accIn')) + s_rate = filter(lambda s: s.type == 'RateGyro', self.sensors) + if (len(s_rate) > 0) and self.rh.port(s_rate[0].name) != None: + connectPorts(self.rh.port(s_rate[0].name), self.acf.port("rateIn")) + if self.kf: + connectPorts(self.kf.port("rpy"), self.acf.port("rpyIn")) + if self.abc: + connectPorts(self.abc.port("basePosOut"), self.acf.port("posIn")) + def activateComps(self): '''!@brief Activate components(plugins) @@ -669,16 +773,20 @@ def getRTCListUnstable(self): ['kf', "KalmanFilter"], ['vs', "VirtualForceSensor"], ['rmfo', "RemoveForceSensorLinkOffset"], + ['octd', "ObjectContactTurnaroundDetector"], ['es', "EmergencyStopper"], + ['rfu', "ReferenceForceUpdater"], ['ic', "ImpedanceController"], ['abc', "AutoBalancer"], ['st', "Stabilizer"], ['co', "CollisionDetector"], ['tc', "TorqueController"], ['te', "ThermoEstimator"], - ['tl', "ThermoLimiter"], ['hes', "EmergencyStopper"], ['el', "SoftErrorLimiter"], + ['tl', "ThermoLimiter"], + ['bp', "Beeper"], + ['acf', "AccelerationFilter"], ['log', "DataLogger"] ] @@ -686,8 +794,7 @@ def getJointAngleControllerList(self): '''!@brief Get list of controller list that need to control joint angles ''' - controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, - self.tc, self.hes, self.el] + controller_list = [self.es, self.ic, self.gc, self.abc, self.st, self.co, self.tc, self.hes, self.el] return filter(lambda c: c != None, controller_list) # only return existing controllers def getRTCInstanceList(self, verbose=True): @@ -804,10 +911,22 @@ def setupLogger(self, maxLength=4000): if self.abc != None: self.connectLoggerPort(self.abc, 'zmpOut') self.connectLoggerPort(self.abc, 'baseTformOut') + self.connectLoggerPort(self.abc, 'qAbc') self.connectLoggerPort(self.abc, 'q') self.connectLoggerPort(self.abc, 'contactStates') self.connectLoggerPort(self.abc, 'controlSwingSupportTime') self.connectLoggerPort(self.abc, 'cogOut') + self.connectLoggerPort(self.abc, 'tmp') + self.connectLoggerPort(self.abc, 'currentLandingPos') + self.connectLoggerPort(self.abc, 'allEEComp') + self.connectLoggerPort(self.abc, 'originRefZmp') + self.connectLoggerPort(self.abc, 'originRefCog') + self.connectLoggerPort(self.abc, 'originRefCogVel') + self.connectLoggerPort(self.abc, 'originNewZmp') + self.connectLoggerPort(self.abc, 'originActZmp') + self.connectLoggerPort(self.abc, 'originActCog') + self.connectLoggerPort(self.abc, 'originActCogVel') + self.connectLoggerPort(self.abc, 'tau') if self.st != None: self.connectLoggerPort(self.st, 'zmp') self.connectLoggerPort(self.st, 'originRefZmp') @@ -826,6 +945,8 @@ def setupLogger(self, maxLength=4000): self.connectLoggerPort(self.st, 'debugData') if self.el != None: self.connectLoggerPort(self.el, 'q') + if self.es != None: + self.connectLoggerPort(self.es, 'q') if self.rh != None: self.connectLoggerPort(self.rh, 'emergencySignal', 'emergencySignal') @@ -837,6 +958,11 @@ def setupLogger(self, maxLength=4000): if self.rmfo != None: for sen in filter(lambda x: x.type == "Force", self.sensors): self.connectLoggerPort(self.rmfo, "off_"+sen.name) + if self.rfu != None: + for sen in filter(lambda x: x.type == "Force", self.sensors): + self.connectLoggerPort(self.rfu, "ref_"+sen.name+"Out") + if self.octd != None: + self.connectLoggerPort(self.octd, "octdData") self.log_svc.clear() ## parallel running log process (outside from rtcd) for saving logs by emergency signal if self.log and (self.log_use_owned_ec or not isinstance(self.log.owned_ecs[0], OpenRTM._objref_ExtTrigExecutionContextService)): @@ -884,7 +1010,7 @@ def waitForRobotHardware(self, robotname="Robot"): print(self.configurator_name + "Exitting.... " + robotname) exit(1) - print(self.configurator_name + "findComps -> RobotHardware : %s isActive? = %s " % (self.rh, self.rh.isActive())) + print(self.configurator_name + "findComps -> %s : %s isActive? = %s " % (self.rh.name(), self.rh, self.rh.isActive())) def checkSimulationMode(self): '''!@brief @@ -906,6 +1032,11 @@ def checkSimulationMode(self): print(self.configurator_name + "simulation_mode : %s" % self.simulation_mode) def waitForRTCManagerAndRoboHardware(self, robotname="Robot", managerhost=nshost): + print("\033[93m%s waitForRTCManagerAndRoboHardware has renamed to " % self.configurator_name + \ + "waitForRTCManagerAndRoboHardware: Please update your code\033[0m") + return self.waitForRTCManagerAndRobotHardware(robotname=robotname, managerhost=managerhost) + + def waitForRTCManagerAndRobotHardware(self, robotname="Robot", managerhost=nshost): '''!@brief Wait for both RTC Manager (waitForRTCManager()) and RobotHardware (waitForRobotHardware()) @@ -969,6 +1100,8 @@ def setJointAngle(self, jname, angle, tm): @param jname str: name of joint @param angle float: In degree. @param tm float: Time to complete. + @rtype bool + @return False upon any problem during execution. ''' radangle = angle / 180.0 * math.pi return self.seq_svc.setJointAngle(jname, radangle, tm) @@ -984,6 +1117,8 @@ def setJointAngles(self, angles, tm): \endverbatim @param angles list of float: In degree. @param tm float: Time to complete. + @rtype bool + @return False upon any problem during execution. ''' ret = [] for angle in angles: @@ -1007,6 +1142,8 @@ def setJointAnglesOfGroup(self, gname, pose, tm, wait=True): @param tm float: Time to complete. @param wait bool: If true, all other subsequent commands wait until the movement commanded by this method call finishes. + @rtype bool + @return False upon any problem during execution. ''' angles = [x / 180.0 * math.pi for x in pose] ret = self.seq_svc.setJointAnglesOfGroup(gname, angles, tm) @@ -1016,15 +1153,17 @@ def setJointAnglesOfGroup(self, gname, pose, tm, wait=True): def setJointAnglesSequence(self, angless, tms): '''!@brief - Set all joint angles. + Set all joint angles. len(angless) should be equal to len(tms). \verbatim NOTE: While this method does not check angle value range, any joints could emit position limit over error, which has not yet been thrown by hrpsys so that there's no way to catch on this python client. Worthwhile opening an enhancement ticket at designated issue tracker. \endverbatim - @param sequence angles list of float: In degree. - @param tm sequence of float: Time to complete, In Second + @param sequential list of angles in float: In rad + @param tm sequential list of time in float: Time to complete, In Second + @rtype bool + @return False upon any problem during execution. ''' for angles in angless: for i in range(len(angles)): @@ -1041,8 +1180,10 @@ def setJointAnglesSequenceOfGroup(self, gname, angless, tms): Worthwhile opening an enhancement ticket at designated issue tracker. \endverbatim @param gname str: Name of the joint group. - @param sequence angles list of float: In degree. - @param tm sequence of float: Time to complete, In Second + @param sequential list of angles in float: In rad + @param tm sequential list of time in float: Time to complete, In Second + @rtype bool + @return False upon any problem during execution. ''' for angles in angless: for i in range(len(angles)): @@ -1093,6 +1234,14 @@ def waitInterpolationOfGroup(self, gname): ''' self.seq_svc.waitInterpolationOfGroup(gname) + def setInterpolationMode(self, mode): + '''!@brief + Set interpolation mode. You may need to import OpenHRP in order to pass an argument. For more info See https://github.com/fkanehiro/hrpsys-base/pull/1012#issue-160802911. + @param mode new interpolation mode. Either { OpenHRP.SequencePlayerService.LINEAR, OpenHRP.SequencePlayerService.HOFFARBIB }. + @return true if set successfully, false otherwise + ''' + return self.seq_svc.setInterpolationMode(mode) + def getJointAngles(self): '''!@brief Returns the commanded joint angle values. @@ -1109,7 +1258,7 @@ def getJointAngles(self): def getCurrentPose(self, lname=None, frame_name=None): '''!@brief - Returns the current physical pose of the specified joint. + Returns the current physical pose of the specified joint in the joint space. cf. getReferencePose that returns commanded value. eg. @@ -1153,7 +1302,7 @@ def getCurrentPose(self, lname=None, frame_name=None): raise RuntimeError("need to specify joint name") if frame_name: lname = lname + ':' + frame_name - if self.fk_version < '315.2.5' and ':' in lname: + if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname: raise RuntimeError('frame_name ('+lname+') is not supported') pose = self.fk_svc.getCurrentPose(lname) if not pose[0]: @@ -1162,7 +1311,7 @@ def getCurrentPose(self, lname=None, frame_name=None): def getCurrentPosition(self, lname=None, frame_name=None): '''!@brief - Returns the current physical position of the specified joint. + Returns the current physical position of the specified joint in Cartesian space. cf. getReferencePosition that returns commanded value. eg. @@ -1230,7 +1379,7 @@ def getCurrentRPY(self, lname, frame_name=None): def getReferencePose(self, lname, frame_name=None): '''!@brief - Returns the current commanded pose of the specified joint. + Returns the current commanded pose of the specified joint in the joint space. cf. getCurrentPose that returns physical pose. @type lname: str @@ -1253,7 +1402,7 @@ def getReferencePose(self, lname, frame_name=None): raise RuntimeError("need to specify joint name") if frame_name: lname = lname + ':' + frame_name - if self.fk_version < '315.2.5' and ':' in lname: + if StrictVersion(self.fk_version) < StrictVersion('315.2.5') and ':' in lname: raise RuntimeError('frame_name ('+lname+') is not supported') pose = self.fk_svc.getReferencePose(lname) if not pose[0]: @@ -1262,7 +1411,7 @@ def getReferencePose(self, lname, frame_name=None): def getReferencePosition(self, lname, frame_name=None): '''!@brief - Returns the current commanded position of the specified joint. + Returns the current commanded position of the specified joint in Cartesian space. cf. getCurrentPosition that returns physical value. @type lname: str @@ -1331,7 +1480,8 @@ def setTargetPose(self, gname, pos, rpy, tm, frame_name=None): @param gname str: Name of the joint group. @param pos list of float: In meter. @param rpy list of float: In radian. - @param tm float: Second to complete the command. + @param tm float: Second to complete the command. This only includes the time taken for execution + (i.e. time for planning and other misc. processes are not considered). @param frame_name str: Name of the frame that this particular command references to. @return bool: False if unreachable. @@ -1352,6 +1502,10 @@ def setTargetPoseRelative(self, gname, eename, dx=0, dy=0, dz=0, '''!@brief Move the end-effector's location relative to its current pose. + Note that because of "relative" nature, the method waits for the commands + run previously to finish. Do not get confused with the "wait" argument, + which regulates all subsequent commands, not previous ones. + For d*, distance arguments are in meter while rotations are in degree. Example usage: The following moves RARM_JOINT5 joint 0.1mm forward @@ -1367,7 +1521,8 @@ def setTargetPoseRelative(self, gname, eename, dx=0, dy=0, dz=0, @param dr float: In radian. @param dp float: In radian. @param dw float: In radian. - @param tm float: Second to complete the command. + @param tm float: Second to complete the command. This only includes the time taken for execution + (i.e. time for planning and other misc. processes are not considered). @param wait bool: If true, all other subsequent commands wait until the movement commanded by this method call finishes. @return bool: False if unreachable. @@ -1581,7 +1736,7 @@ def getActualState(self): Get actual states of the robot that includes current reference joint angles and joint torques. @return: This returns actual states of the robot, which is defined in RobotHardware.idl - (https://github.com/fkanehiro/hrpsys-base/blob/master/idl/RobotHardwareService.idl#L33) + (found at https://github.com/fkanehiro/hrpsys-base/blob/3fd7671de5129101a4ade3f98e2eac39fd6b26c0/idl/RobotHardwareService.idl#L32_L57 as of version 315.11.0) \verbatim /** * @brief status of the robot @@ -1833,33 +1988,46 @@ def playPattern(self, jointangles, rpy, zmp, tm): Play motion pattern using a given trajectory that is represented by a list of joint angles, rpy, zmp and time. - @param jointangles list of list of float: - The whole list represents a trajectory. Each element - of the 1st degree in the list consists of the joint - angles. + @type jointangles: [[float]] + @param jointangles: Sequence of the sets of joint angles in radian. + The whole list represents a trajectory. Each element + of the 1st degree in the list consists of the joint angles. @param rpy list of float: Orientation in rpy. @param zmp list of float: TODO: description - @param tm float: Time to complete the task. + @param tm float: Second to complete the command. This only includes the time taken for execution + (i.e. time for planning and other misc. processes are not considered). @return bool: ''' return self.seq_svc.playPattern(jointangles, rpy, zmp, tm) def playPatternOfGroup(self, gname, jointangles, tm): '''!@brief - Play motion pattern using a given trajectory that is represented by - a list of joint angles. + Play motion pattern using a set of given trajectories that are represented by + lists of joint angles. Each trajectory aims to run within the specified time (tm), + and there's no slow down between trajectories unless the next one is the last. + + Example: + self.playPatternOfGroup('larm', + [[0.0, 0.0, -2.2689280275926285, 0.0, 0.0, 0.0], + [0.0, 0.0, -1.9198621771937625, 0.0, 0.0, 0.0], + [0.0, 0.0, -1.5707963, 0.0, 0.0, 0.0]], + [3, 3, 10]) @param gname str: Name of the joint group. - @param jointangles list of list of float: - The whole list represents a trajectory. Each element - of the 1st degree in the list consists of the joint - angles. To illustrate: - - [[a0-0, a0-1,...,a0-n], # a)ngle. 1st path in trajectory - [a1-0, a1-1,...,a1-n], # 2nd path in the trajectory. - : - [am-0, am-1,...,am-n]] # mth path in the trajectory - @param tm float: Time to complete the task. + @type jointangles: [[float]] + @param jointangles: Sequence of the sets of joint angles in radian. + The whole list represents a trajectory. Each element + of the 1st degree in the list consists of the joint + angles. To illustrate: + + [[a0-0, a0-1,...,a0-n], # a)ngle. 1st path in trajectory + [a1-0, a1-1,...,a1-n], # 2nd path in the trajectory. + : + [am-0, am-1,...,am-n]] # mth path in the trajectory + @type tm: [float] + @param tm float: Sequence of the time values to complete the task, + each element of which corresponds to a set of jointangles + in the same order. @return bool: ''' return self.seq_svc.playPatternOfGroup(gname, jointangles, tm) @@ -1925,13 +2093,13 @@ def startStabilizer(self): '''!@brief Start Stabilzier mode ''' - self.st_svc.startStabilizer() + self.abc_svc.startStabilizer() def stopStabilizer(self): '''!@brief Stop Stabilzier mode ''' - self.st_svc.stopStabilizer() + self.abc_svc.stopStabilizer() def startImpedance_315_4(self, arm, M_p = 100.0, @@ -1952,6 +2120,8 @@ def startImpedance_315_4(self, arm, start impedance mode @type arm: str name of artm to be controlled, this must be initialized using setSelfGroups() + @param force_gain, moment_gain: multipliers to the eef offset position vel_p and orientation vel_r. + 3-dimensional vector (then converted internally into a diagonal matrix). ''' r, p = self.ic_svc.getImpedanceControllerParam(arm) if not r: @@ -1981,45 +2151,108 @@ def stopImpedance_315_4(self, arm): return self.ic_svc.stopImpedanceController(arm) def startImpedance(self, arm, **kwargs): - if self.hrpsys_version and self.hrpsys_version < '315.2.0': + '''!@brief + Enable the ImpedanceController RT component. + This method internally calls startImpedance-*, hrpsys version-specific method. + + @requires: hrpsys version greather than 315.2.0. + @requires: ImpedanceController RTC to be activated on the robot's controller. + @change: From 315.2.0 onward, following arguments are dropped and can be set by + self.seq_svc.setWrenches instead of this method. + See an example at https://github.com/fkanehiro/hrpsys-base/pull/434/files#diff-6204f002204dd9ae80f203901f155fa9R44: + + - ref_force[fx, fy, fz] (unit: N) and ref_moment[tx, ty, tz] (unit: Nm) can be set via self.seq_svc.setWrenches. For example: + + self.seq_svc.setWrenches([0, 0, 0, 0, 0, 0, + fx, fy, fz, tx, ty, tz, + 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0,]) + + setWrenches takes 6 values per sensor, so the robot in the example above has 4 sensors where each line represents a sensor. + See this link (https://github.com/fkanehiro/hrpsys-base/pull/434/files) for a concrete example. + + @param arm: Name of the kinematic group (i.e. self.Groups[n][0]). + @param kwargs: This varies depending on the version of hrpsys your robot's controller runs on + (which you can find by "self.hrpsys_version" command). For instance, if your + hrpsys is 315.10.1, refer to "startImpedance_315_4" method. + ''' + if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'): print(self.configurator_name + '\033[31mstartImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m') else: self.startImpedance_315_4(arm, **kwargs) def stopImpedance(self, arm): - if self.hrpsys_version and self.hrpsys_version < '315.2.0': + if self.hrpsys_version and StrictVersion(self.hrpsys_version) < StrictVersion('315.2.0'): print(self.configurator_name + '\033[31mstopImpedance: Try to connect unsupported RTC' + str(self.hrpsys_version) + '\033[0m') else: self.stopImpedance_315_4(arm) - def startDefaultUnstableControllers (self, ic_limbs=["rarm", "larm"], abc_limbs=None): + def startDefaultUnstableControllers (self, ic_limbs=[], abc_limbs=[]): '''!@brief Start default unstable RTCs controller mode. - Currently Stabilzier, AutoBalancer, and ImpedanceController are started. - ''' + Currently Stabilzier, AutoBalancer, and ImpedanceController are started based on "Groups" setting. + If ic_limbs or abc_limbs is not specified, default setting is set according to "Groups". + By default, + If the robot has an arm, start impedance for the arm. + If the robot has a leg, start st and autobalancer. + autobalancer's fixed limbs are all existing arms and legs. + Use cases: + Biped robot (leg only) : no impedance, start st, start autobalancer with ["rleg", "lleg"]. + Dual-arm robot (arm only) : start impedance ["rarm", "larm"], no st, no autobalancer. + Dual-arm+biped robot (=humanoid robot) : start impedance ["rarm", "larm"], start st, start autobalancer with ["rleg", "lleg", "rarm", "larm"]. + Quadruped robot : same as "humanoid robot" by default. + ''' + print(self.configurator_name + "Start Default Unstable Controllers") + # Check robot setting + is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups)) + # Select all arms in "Groups" for impedance as a default setting + if not ic_limbs: + ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups)) + # Select all arms/legs in "Groups" for autobalancer as a default setting + if not abc_limbs: + abc_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(leg|arm)", x[0]), self.Groups)) + # Start controllers for limb in ic_limbs: self.ic_svc.startImpedanceControllerNoWait(limb) - if abc_limbs==None: - if self.Groups != None and "rarm" in map (lambda x : x[0], self.Groups) and "larm" in map (lambda x : x[0], self.Groups): - abc_limbs=["rleg", "lleg", "rarm", "larm"] - else: - abc_limbs=["rleg", "lleg"] - self.startAutoBalancer(abc_limbs) - self.startStabilizer() + if is_legged_robot: + self.startAutoBalancer(abc_limbs) + self.startStabilizer() for limb in ic_limbs: self.ic_svc.waitImpedanceControllerTransition(limb) + # Print + if is_legged_robot: + print(self.configurator_name + " Start AutoBalancer = "+str(abc_limbs)) + print(self.configurator_name + " Start Stabilizer") + if len(ic_limbs) != 0: + print(self.configurator_name + " Start ImpedanceController = "+str(ic_limbs)) - def stopDefaultUnstableControllers (self, ic_limbs=["rarm", "larm"]): + def stopDefaultUnstableControllers (self, ic_limbs=[]): '''!@brief Stop default unstable RTCs controller mode. - Currently Stabilzier, AutoBalancer, and ImpedanceController are stopped. - ''' - self.stopStabilizer() + Currently Stabilzier, AutoBalancer, and ImpedanceController are stopped based on "Groups" setting. + Please see documentation of startDefaultUnstableControllers(). + ''' + print(self.configurator_name + "Stop Default Unstable Controllers") + # Check robot setting + is_legged_robot = map(lambda x: x[0], filter(lambda x : re.match(".*leg", x[0]), self.Groups)) + # Select all arms in "Groups" for impedance as a default setting + if not ic_limbs: + ic_limbs = map(lambda x: x[0], filter(lambda x : re.match(".*(arm)", x[0]), self.Groups)) + # Stop controllers + if is_legged_robot: + self.stopStabilizer() for limb in ic_limbs: self.ic_svc.stopImpedanceControllerNoWait(limb) - self.stopAutoBalancer() + if is_legged_robot: + self.stopAutoBalancer() for limb in ic_limbs: self.ic_svc.waitImpedanceControllerTransition(limb) + # Print + if is_legged_robot: + print(self.configurator_name + " Stop AutoBalancer") + print(self.configurator_name + " Stop Stabilizer") + if len(ic_limbs) != 0: + print(self.configurator_name + " Stop ImpedanceController = "+str(ic_limbs)) def setFootSteps(self, footstep, overwrite_fs_idx = 0): '''!@brief @@ -2038,6 +2271,32 @@ def setFootStepsWithParam(self, footstep, stepparams, overwrite_fs_idx = 0): ''' self.abc_svc.setFootStepsWithParam(footstep, stepparams, overwrite_fs_idx) + def clearJointAngles(self): + '''!@brief + Cancels the commanded sequence of joint angle, by overwriting the command by the values of instant the method was invoked. + Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual. + @return bool + ''' + return self.seq_svc.clearJointAngles() + + def clearJointAnglesOfGroup(self, gname): + '''!@brief + Cancels the commanded sequence of joint angle for the specified joint group, by overwriting the command by the values of instant the method was invoked. + Note that this only cancels the queue once. Icoming commands after this method is called will be processed as usual. + @param gname: Name of the joint group. + @return bool + ''' + return self.seq_svc.clearJointAnglesOfGroup(gname) + + def removeForceSensorOffsetRMFO(self, sensor_names=[], tm=8.0): + '''!@brief + remove force sensor offset by RemoveForceSensorOffset (RMFO) RTC. + @param sensor_names : list of sensor names to be calibrated. If not specified, all sensors are calibrated by default. + @param tm : calibration time[s]. 8.0[s] by default. + @return bool : true if set successfully, false otherwise + ''' + return self.rmfo_svc.removeForceSensorOffset(sensor_names, tm) + # ## # ## initialize # ## @@ -2057,7 +2316,7 @@ def init(self, robotname="Robot", url=""): print(self.configurator_name + "start hrpsys") print(self.configurator_name + "finding RTCManager and RobotHardware") - self.waitForRTCManagerAndRoboHardware(robotname) + self.waitForRTCManagerAndRobotHardware(robotname) self.sensors = self.getSensors(url) print(self.configurator_name + "creating components") diff --git a/python/rtm.py b/python/rtm.py index 0636da3e8ca..2ac1a7fc175 100644 --- a/python/rtm.py +++ b/python/rtm.py @@ -18,6 +18,8 @@ rootnc = None nshost = None nsport = None +mgrhost = None +mgrport = None ## # \brief wrapper class of RT component @@ -78,6 +80,13 @@ def setConfiguration(self, nvlist): def setProperty(self, name, value): return self.setConfiguration([[name, value]]) + ## + # \brief get name-value list of the default configuration set + # \param self this object + # \return name-value list of the default configuration set + def getProperties(self): + return getConfiguration(self.ref) + ## # \brief get value of the property in the default configuration set # \param self this object @@ -105,13 +114,21 @@ def start(self, ec=None, timeout=3.0): if ec == None: ec = self.ec if ec != None: - ec.activate_component(self.ref) + if self.isActive(ec): + return True + ret = ec.activate_component(self.ref) + if ret != RTC.RTC_OK: + print ('[rtm.py] \033[31m Failed to start %s(%s)\033[0m' % \ + (self.name(), ret)) + return False tm = 0 while tm < timeout: if self.isActive(ec): return True time.sleep(0.01) tm += 0.01 + print ('[rtm.py] \033[31m Failed to start %s(timeout)\033[0m' % \ + self.name()) return False ## @@ -124,16 +141,35 @@ def stop(self, ec=None, timeout=3.0): if ec == None: ec = self.ec if ec != None: - ec.deactivate_component(self.ref) + if self.isInactive(ec): + return True + ret = ec.deactivate_component(self.ref) + if ret != RTC.RTC_OK: + print ('[rtm.py] \033[31m Failed to stop %s(%s)\033[0m' % \ + (self.name(), ret)) + return False tm = 0 while tm < timeout: if self.isInactive(ec): return True time.sleep(0.01) tm += 0.01 + print ('[rtm.py] \033[31m Failed to stop %s(timeout)\033[0m' % \ + self.name()) return False ## + # \brief reset this component + # \param self this object + # \param ec execution context used to reset this component + # \return True if reseted successfully, False otherwise + def reset(self, ec=None, timeout=3.0): + if self.getLifeCycleState(ec) != RTC.ERROR_STATE: + return True + if ec == None: + ec = self.ec + return ec.reset_component(self.ref) == RTC.RTC_OK + ## # \brief get life cycle state of the main execution context # \param self this object # \param ec execution context from which life cycle state is obtained @@ -277,7 +313,7 @@ def unbindObject(name, kind): # \brief initialize ORB # def initCORBA(): - global rootnc, orb, nshost, nsport + global rootnc, orb, nshost, nsport, mgrhost, mgrport # from omniorb's document # When CORBA::ORB_init() is called, the value for each configuration @@ -286,17 +322,55 @@ def initCORBA(): # Environment variables # so init_CORBA will follow this order # first check command line argument - try: - n = sys.argv.index('-ORBInitRef') - (nshost, nsport) = re.match( - 'NameService=corbaloc:iiop:(\w+):(\d+)/NameService', sys.argv[n + 1]).group(1, 2) - except: - if not nshost: - nshost = socket.gethostname() - if not nsport: - nsport = 15005 - print("configuration ORB with %s:%s"%(nshost, nsport)) + rtm_argv = [sys.argv[0]] # extract rtm related args only + for i in range(len(sys.argv)): + if sys.argv[i] == '-o': + rtm_argv.extend(['-o', sys.argv[i+1]]) + + mc = OpenRTM_aist.ManagerConfig(); + mc.parseArgs(rtm_argv) + + if nshost != None: # these values can be set via other script like "import rtm; rtm.nshost=XXX" + print("\033[34m[rtm.py] nshost already set as " + str(nshost) + "\033[0m") + else: + try: + nshost = mc._argprop.getProperty("corba.nameservers").split(":")[0] + if not nshost: + raise + except: + nshost = socket.gethostname() # default + print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nshost) + " as nshost \033[0m") + + if nsport != None: + print("\033[34m[rtm.py] nsport already set as " + str(nsport) + "\033[0m") + else: + try: + nsport = int(mc._argprop.getProperty("corba.nameservers").split(":")[1]) + if not nsport: + raise + except: + nsport = 15005 # default + print("\033[34m[rtm.py] Failed to parse corba.nameservers, use " + str(nsport) + " as nsport \033[0m") + + if mgrhost != None: + print("\033[34m[rtm.py] mgrhost already set as " + str(mgrhost) + "\033[0m") + else: + mgrhost = nshost + + if mgrport != None: + print("\033[34m[rtm.py] mgrport already set as " + str(mgrport) + "\033[0m") + else: + try: + mgrport = int(mc._argprop.getProperty("corba.master_manager").split(":")[1]) + if not mgrport: + raise + except: + mgrport = 2810 # default + print("\033[34m[rtm.py] Failed to parse corba.master_manager, use " + str(mgrport) + "\033[0m") + + print("\033[34m[rtm.py] configuration ORB with %s:%s\033[0m"%(nshost, nsport)) + print("\033[34m[rtm.py] configuration RTCManager with %s:%s\033[0m"%(mgrhost, mgrport)) os.environ['ORBInitRef'] = 'NameService=corbaloc:iiop:%s:%s/NameService' % \ (nshost, nsport) @@ -309,9 +383,13 @@ def initCORBA(): sys.exit('[ERROR] Invalide Name (hostname=%s).\n' % (nshost) + 'Make sure the hostname is correct.\n' + str(e)) except omniORB.CORBA.TRANSIENT: - _, e, _ = sys.exc_info() - sys.exit('[ERROR] Connection Failed with the Nameserver (hostname=%s port=%s).\n' % (nshost, nsport) + - 'Make sure the hostname is correct and the Nameserver is running.\n' + str(e)) + try: + nameserver = orb.string_to_object('corbaloc:iiop:%s:%s/NameService'%(nshost, nsport)) + rootnc = nameserver._narrow(CosNaming.NamingContext) + except: + _, e, _ = sys.exc_info() + sys.exit('[ERROR] Connection Failed with the Nameserver (hostname=%s port=%s).\n' % (nshost, nsport) + + 'Make sure the hostname is correct and the Nameserver is running.\n' + str(e)) except Exception: _, e, _ = sys.exc_info() print(str(e)) @@ -382,8 +460,9 @@ def getManagerFromNS(hostname, mgr=None): return mgr def getManagerDirectly(hostname, mgr=None): - global orb - corbaloc = "corbaloc:iiop:" + hostname + ":2810/manager" + global orb, mgrport + corbaloc = "corbaloc:iiop:" + hostname + ":" + str(mgrport) + "/manager" + print("\033[34m[rtm.py] trying to findRTCManager on port" + str(mgrport) + "\033[0m") try: obj = orb.string_to_object(corbaloc) mgr = RTCmanager(obj._narrow(RTM.Manager)) @@ -526,7 +605,7 @@ def dataTypeOfPort(port): # \param bufferlength length of data buffer # \param rate communication rate for periodic mode[Hz] # -def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000, pushpolicy="new"): +def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength=1, rate=1000, pushpolicy="new", interfaceType="corba_cdr"): if not isinstance(inPs, list): inPs = [inPs] if not outP: @@ -538,7 +617,7 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength print('[rtm.py] \033[31m Failed to connect %s to %s(%s)\033[0m' % \ (outP.get_port_profile().name, inP, inPs)) continue - if isConnected(outP, inP) == True and False: + if isConnected(outP, inP) == True: print('[rtm.py] %s and %s are already connected' % \ (outP.get_port_profile().name, inP.get_port_profile().name)) continue @@ -546,7 +625,7 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength print('[rtm.py] \033[31m %s and %s have different data types\033[0m' % \ (outP.get_port_profile().name, inP.get_port_profile().name)) continue - nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr")) + nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any(interfaceType)) nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any(dataflow)) nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any(subscription)) nv4 = SDOPackage.NameValue("dataport.buffer.length", any.to_any(str(bufferlength))) @@ -556,7 +635,7 @@ def connectPorts(outP, inPs, subscription="flush", dataflow="Push", bufferlength con_prof = RTC.ConnectorProfile("connector0", "", [outP, inP], [nv1, nv2, nv3, nv4, nv5, nv6, nv7]) print('[rtm.py] Connect ' + outP.get_port_profile().name + ' - ' + \ - inP.get_port_profile().name) + inP.get_port_profile().name+' (dataflow_type='+dataflow+', subscription_type='+ subscription+', bufferlength='+str(bufferlength)+', push_rate='+str(rate)+', push_policy='+pushpolicy+')') ret, prof = inP.connect(con_prof) if ret != RTC.RTC_OK: print("failed to connect") @@ -580,11 +659,18 @@ def data2cdr(data): # def classFromString(fullname): component_path = fullname.split('.') - package_path = component_path[:-1] - package_name = ".".join(package_path) - class_name = component_path[-1] - __import__(str(package_name)) - return getattr(sys.modules[package_name], class_name) + package_name = component_path[0] + component_path = component_path[1:] + attr = None + while component_path: + class_name = component_path[0] + component_path = component_path[1:] + if attr: + attr = getattr(attr, class_name) + else: + __import__(str(package_name)) + attr = getattr(sys.modules[package_name], class_name) + return attr ## # \brief convert data from CDR format @@ -595,6 +681,10 @@ def classFromString(fullname): def cdr2data(cdr, classname): return cdrUnmarshal(any.to_any(classFromString(classname)).typecode(), cdr, True) + + +connector_list = [] + ## # \brief write data to a data port # \param port reference of data port @@ -605,14 +695,38 @@ def cdr2data(cdr, classname): # the connection must be disconnected by a user # def writeDataPort(port, data, tm=1.0, disconnect=True): - nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr")) - nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any("Push")) - nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any("flush")) - con_prof = RTC.ConnectorProfile("connector0", "", [port], [nv1, nv2, nv3]) - ret, prof = port.connect(con_prof) - if ret != RTC.RTC_OK: - print("failed to connect") - return None + global connector_list, orb + + connector_name = "writeDataPort" + + + + prof = None + + for p in connector_list: + if p["port"]._is_equivalent(port): + if port.get_connector_profile(p["prof"].connector_id).name == connector_name: + prof = p["prof"] + else: + connector_list.remove(p) + + + + + if prof is None: + nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr")) + nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any("Push")) + nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any("flush")) + con_prof = RTC.ConnectorProfile(connector_name, "", [port], [nv1, nv2, nv3]) + + ret, prof = port.connect(con_prof) + + if ret != RTC.RTC_OK: + print("failed to connect") + return None + connector_list.append({"port":port,"prof":prof}) + + for p in prof.properties: if p.name == 'dataport.corba_cdr.inport_ior': ior = any.from_any(p.value) @@ -624,32 +738,60 @@ def writeDataPort(port, data, tm=1.0, disconnect=True): if disconnect: time.sleep(tm) port.disconnect(prof.connector_id) + for p in connector_list: + if prof.connector_id == p["prof"].connector_id: + connector_list.remove(p) else: return prof.connector_id return None + + ## # \brief read data from a data port # \param port reference of data port # \param timeout timeout[s] # \return data # -def readDataPort(port, timeout=1.0): +def readDataPort(port, timeout=1.0, disconnect=True): + global connector_list, orb + + + connector_name = "readDataPort" + prof = None + for p in connector_list: + if p["port"]._is_equivalent(port): + if port.get_connector_profile(p["prof"].connector_id).name == connector_name: + prof = p["prof"] + else: + connector_list.remove(p) + + + + + + pprof = port.get_port_profile() for prop in pprof.properties: if prop.name == "dataport.data_type": classname = any.from_any(prop.value) - break - nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr")) - nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any("Pull")) - nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any("flush")) - con_prof = RTC.ConnectorProfile("connector0", "", [port], [nv1, nv2, nv3]) - ret, prof = port.connect(con_prof) - if ret != RTC.RTC_OK: - print("failed to connect") - return None + + if prof is None: + + nv1 = SDOPackage.NameValue("dataport.interface_type", any.to_any("corba_cdr")) + nv2 = SDOPackage.NameValue("dataport.dataflow_type", any.to_any("Pull")) + nv3 = SDOPackage.NameValue("dataport.subscription_type", any.to_any("flush")) + con_prof = RTC.ConnectorProfile(connector_name, "", [port], [nv1, nv2, nv3]) + + ret, prof = port.connect(con_prof) + + if ret != RTC.RTC_OK: + print("failed to connect") + return None + + connector_list.append({"port":port,"prof":prof}) + for p in prof.properties: - # print(p.name) if p.name == 'dataport.corba_cdr.outport_ior': ior = any.from_any(p.value) obj = orb.string_to_object(ior) @@ -659,7 +801,12 @@ def readDataPort(port, timeout=1.0): try: ret, data = outport.get() if ret == OpenRTM.PORT_OK: - port.disconnect(prof.connector_id) + if disconnect: + port.disconnect(prof.connector_id) + for p in connector_list: + if prof.connector_id == p["prof"].connector_id: + connector_list.remove(p) + tokens = classname.split(':') if len(tokens) == 3: # for 1.1? classname = tokens[1].replace('/', '.') @@ -668,10 +815,19 @@ def readDataPort(port, timeout=1.0): pass time.sleep(0.1) tm = tm + 0.1 + - port.disconnect(prof.connector_id) + return None +## +# \brief +# +def deleteAllConnector(): + global connector_list + for port in connector_list: + port["port"].disconnect(port["prof"].connector_id) + del connector_list[:] ## # \brief get a service of RT component @@ -741,6 +897,23 @@ def setConfiguration(rtc, nvlist): cfg.activate_configuration_set('default') return ret +## +# \brief get default configuration set +# \param rtc IOR of RT component +# \return default configuration set +# +def getConfiguration(rtc): + cfg = rtc.get_configuration() + cfgsets = cfg.get_configuration_sets() + if len(cfgsets) == 0: + print("configuration set is not found") + return None + ret = {} + for nv in cfgsets[0].configuration_data: + ret[nv.name] = any.from_any(nv.value) + return ret + + ## # \brief narrow ior # \param ior ior diff --git a/rtc/AccelerationChecker/AccelerationChecker.cpp b/rtc/AccelerationChecker/AccelerationChecker.cpp index fafdb5151a7..a9aaa538180 100644 --- a/rtc/AccelerationChecker/AccelerationChecker.cpp +++ b/rtc/AccelerationChecker/AccelerationChecker.cpp @@ -11,6 +11,7 @@ #include #include #include "AccelerationChecker.h" +#include "hrpsys/idl/RobotHardwareService.hh" // Module specification // @@ -38,6 +39,7 @@ AccelerationChecker::AccelerationChecker(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // m_qIn("qIn", m_q), + m_servoStateIn("servoState", m_servoState), m_qOut("qOut", m_q), // dummy(0) @@ -64,6 +66,7 @@ RTC::ReturnCode_t AccelerationChecker::onInitialize() // // Set InPort buffers addInPort("qIn", m_qIn); + addInPort("servoState", m_servoStateIn); // Set OutPort buffer addOutPort("qOut", m_qOut); @@ -129,6 +132,7 @@ RTC::ReturnCode_t AccelerationChecker::onExecute(RTC::UniqueId ec_id) if (m_qIn.isNew()){ m_qIn.read(); + while(m_servoStateIn.isNew()) m_servoStateIn.read(); if (!m_dq.data.length()){ // first time m_qOld.data.length(m_q.data.length()); m_qOld = m_q; @@ -143,10 +147,18 @@ RTC::ReturnCode_t AccelerationChecker::onExecute(RTC::UniqueId ec_id) for (unsigned int i=0; i m_ddqMax.data[i]) m_ddqMax.data[i] = fabs(ddq); - if (fabs(ddq) > m_thd){ - std::cout << std::fixed << std::setprecision(3) << "[" - << (double)coil::gettimeofday() + if (servo && fabs(ddq) > m_thd){ + time_t now = time(NULL); + struct tm *tm_now = localtime(&now); + char *datetime = asctime(tm_now); + datetime[strlen(datetime)-1] = '\0'; + std::cout << "[" + << datetime << "] Warning: too big joint acceleration for " << i << "th joint(" << ddq << "[rad/m^2])" << std::endl; } diff --git a/rtc/AccelerationChecker/AccelerationChecker.h b/rtc/AccelerationChecker/AccelerationChecker.h index 5d34d311be3..fea5081066c 100644 --- a/rtc/AccelerationChecker/AccelerationChecker.h +++ b/rtc/AccelerationChecker/AccelerationChecker.h @@ -10,9 +10,11 @@ #ifndef ACCELERATION_CHECKER_H #define ACCELERATION_CHECKER_H +#include #include #include #include +#include #include #include #include @@ -102,10 +104,12 @@ class AccelerationChecker // TimedDoubleSeq m_q; + OpenHRP::TimedLongSeqSeq m_servoState; // DataInPort declaration // InPort m_qIn; + InPort m_servoStateIn; // diff --git a/rtc/AccelerationChecker/CMakeLists.txt b/rtc/AccelerationChecker/CMakeLists.txt index 9e7a02fd3ce..931336e54ae 100644 --- a/rtc/AccelerationChecker/CMakeLists.txt +++ b/rtc/AccelerationChecker/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(AccelerationCheckerComp ${libs}) set(target AccelerationChecker AccelerationCheckerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/AccelerationFilter/AccelerationFilter.cpp b/rtc/AccelerationFilter/AccelerationFilter.cpp new file mode 100644 index 00000000000..2b3fe406965 --- /dev/null +++ b/rtc/AccelerationFilter/AccelerationFilter.cpp @@ -0,0 +1,351 @@ +// -*- C++ -*- +/*! + * @file AccelerationFilter.cpp * @brief Acceleration Filter component * $Date$ + * + * $Id$ + */ +#include "AccelerationFilter.h" + +typedef coil::Guard Guard; +// Module specification +// +static const char* accelerationfilter_spec[] = + { + "implementation_id", "AccelerationFilter", + "type_name", "AccelerationFilter", + "description", "Acceleration Filter component", + "version", "1.0", + "vendor", "JSK", + "category", "example", + "activity_type", "SPORADIC", + "kind", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "" + }; +// + +AccelerationFilter::AccelerationFilter(RTC::Manager* manager) + // + : RTC::DataFlowComponentBase(manager), + m_accInIn("accIn", m_accIn), + m_rateInIn("rateIn", m_rateIn), + m_rpyInIn("rpyIn", m_rpyIn), + m_posInIn("posIn", m_posIn), + m_velOutOut("velOut", m_velOut), + //m_posOutOut("posOut", m_posOut), + m_AccelerationFilterServicePort("AccelerationFilterService"), + // + m_use_filter_bool(false) +{ +} + +AccelerationFilter::~AccelerationFilter() +{ +} + + +RTC::ReturnCode_t AccelerationFilter::onInitialize() +{ + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("accIn", m_accInIn); + addInPort("rateIn", m_rateInIn); + addInPort("rpyIn", m_rpyInIn); + addInPort("posIn", m_posInIn); + + // Set OutPort buffer + addOutPort("velOut", m_velOutOut); + //addOutPort("posOut", m_posOutOut); + + // Set service provider to Ports + m_AccelerationFilterServicePort.registerProvider("service0", "AccelerationFilterService", m_service0); + m_service0.setInstance(this); + + // Set service consumers to Ports + + // Set CORBA Service Ports + addPort(m_AccelerationFilterServicePort); + + // + + // + // Bind variables and configuration variable + RTC::Properties& prop = getProperties(); + if ( ! coil::stringTo(m_dt, prop["dt"].c_str()) ) { + std::cerr << "[" << m_profile.instance_name << "] failed to get dt" << std::endl; + return RTC::RTC_ERROR; + } + + // read gravity param + double param_gravity = 9.80665; + if ( ! coil::stringTo(m_gravity, prop["gravity"].c_str()) ) { + param_gravity = m_gravity = 9.80665; + } + std::cerr << "[" << m_profile.instance_name << "] gravity : " << m_gravity << std::endl; + + // read filter param + { + coil::vstring filter_str = coil::split(prop["iir_filter_setting"], ","); + if (filter_str.size() > 2) { + int dim = (filter_str.size() - 1)/2; + std::vector bb; + std::vector aa; + for(int i = 0; i < dim + 1; i++) { + double val = -1; + coil::stringTo(val, filter_str[i].c_str()); + bb.push_back(val); + } + for(int i = 0; i < filter_str.size() - dim - 1; i++) { + double val = -1; + coil::stringTo(val, filter_str[dim+1+i].c_str()); + aa.push_back(val); + } + if (aa.size() > 0 && bb.size() > 0) { + m_use_filter_bool = true; + std::cerr << "[" << m_profile.instance_name << "] pass filter_param : " << std::endl; + std::cerr << "B = ["; + for(int i = 0; i < bb.size(); i++) { + std::cerr << " " << bb[i]; + } + std::cerr << "]" << std::endl; + std::cerr << "A = ["; + for(int i = 0; i < aa.size(); i++) { + std::cerr << " " << aa[i]; + } + std::cerr << "]" << std::endl; + for (int i = 0; i < 3; i++) { + IIRFilterPtr fl(new IIRFilter(std::string(m_profile.instance_name))); + fl->setParameter(dim, aa, bb); + fl->reset(param_gravity); + m_filters.push_back(fl); + } + } + } + } + + // + return RTC::RTC_OK; +} + + +/* +RTC::ReturnCode_t AccelerationFilter::onFinalize() +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t AccelerationFilter::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t AccelerationFilter::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t AccelerationFilter::onActivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} +RTC::ReturnCode_t AccelerationFilter::onDeactivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl; + // reset filter + return RTC::RTC_OK; +} + + +RTC::ReturnCode_t AccelerationFilter::onExecute(RTC::UniqueId ec_id) +{ + if (m_rpyInIn.isNew()) { + m_rpyInIn.read(); + } + if (m_rateInIn.isNew()) { + m_rateInIn.read(); + } + // calc expected velocity from AutoBalancer + hrp::Vector3 expected_vel; + if (m_posInIn.isNew()) { + m_posInIn.read(); + hrp::Vector3 pos(m_posIn.data.x, m_posIn.data.y, m_posIn.data.z); + expected_vel = pos - m_previous_pos; + expected_vel /= m_dt; + m_previous_pos = pos; + } + + // + if (m_accInIn.isNew()) { + Guard guard(m_mutex); + + m_accInIn.read(); + hrp::Vector3 acc(m_accIn.data.ax, m_accIn.data.ay, m_accIn.data.az); + hrp::Matrix33 imuR = hrp::rotFromRpy(m_rpyIn.data.r, + m_rpyIn.data.p, + m_rpyIn.data.y); + hrp::Vector3 gravity(0, 0, - m_gravity); + hrp::Vector3 acc_wo_g = imuR * acc + gravity; + + for (int i = 0; i < 3; i++) { + if (m_use_filter_bool) { + double filtered_acc = m_filters[i]->passFilter(acc_wo_g[i]); + m_global_vel[i] += filtered_acc * m_dt; + } else { + m_global_vel[i] += acc_wo_g[i] * m_dt; + } + } + + hrp::Vector3 _result_vel = imuR.inverse() * m_global_vel; // result should be described in sensor coords + + m_velOut.data.x = _result_vel[0]; + m_velOut.data.y = _result_vel[1]; + m_velOut.data.z = _result_vel[2]; + m_velOutOut.write(); + } + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t AccelerationFilter::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t AccelerationFilter::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t AccelerationFilter::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t AccelerationFilter::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t AccelerationFilter::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +bool AccelerationFilter::resetFilter(const OpenHRP::AccelerationFilterService::ControlMode &mode, + const double *vel) +{ + Guard guard(m_mutex); + switch(mode) { + case OpenHRP::AccelerationFilterService::MODE_ZERO_VELOCITY: + m_global_vel[0] = 0; + m_global_vel[1] = 0; + m_global_vel[2] = 0; + break; + case OpenHRP::AccelerationFilterService::MODE_RELATIVE_GLOBAL_VELOCITY: + m_global_vel[0] += vel[0]; + m_global_vel[1] += vel[1]; + m_global_vel[2] += vel[2]; + break; + case OpenHRP::AccelerationFilterService::MODE_ABSOLUTE_GLOBAL_VELOCITY: + m_global_vel[0] = vel[0]; + m_global_vel[1] = vel[1]; + m_global_vel[2] = vel[2]; + break; + case OpenHRP::AccelerationFilterService::MODE_RELATIVE_LOCAL_VELOCITY: + { + hrp::Matrix33 imuR = hrp::rotFromRpy(m_rpyIn.data.r, + m_rpyIn.data.p, + m_rpyIn.data.y); + hrp::Vector3 in_vel(vel[0], vel[1], vel[2]); + hrp::Vector3 g_vel = imuR * in_vel; + m_global_vel += g_vel; + } + break; + case OpenHRP::AccelerationFilterService::MODE_ABSOLUTE_LOCAL_VELOCITY: + { + hrp::Matrix33 imuR = hrp::rotFromRpy(m_rpyIn.data.r, + m_rpyIn.data.p, + m_rpyIn.data.y); + hrp::Vector3 in_vel(vel[0], vel[1], vel[2]); + hrp::Vector3 g_vel = imuR * in_vel; + m_global_vel = g_vel; + } + break; + default: + break; + } + return true; +} + +bool AccelerationFilter::setParam(const ::OpenHRP::AccelerationFilterService::AccelerationFilterParam& i_param) +{ + Guard guard(m_mutex); + m_gravity = i_param.gravity; + + if(i_param.filter_param.length() > 1) { + int dim; + std::vector A; + std::vector B; + dim = (i_param.filter_param.length() - 1)/2; + for(int i = 0; i < dim + 1; i++) { + B.push_back(i_param.filter_param[i]); + } + for(int i = 0; i < i_param.filter_param.length() - dim - 1; i++) { + A.push_back(i_param.filter_param[dim+1+i]); + } + m_filters.resize(0); + for(int i = 0; i < 3; i++) { + IIRFilterPtr fl(new IIRFilter); + fl->setParameter(dim, A, B); + m_filters.push_back(fl); + } + m_use_filter_bool = i_param.use_filter; + } + return true; +} + +bool AccelerationFilter::getParam(::OpenHRP::AccelerationFilterService::AccelerationFilterParam &i_param) +{ + i_param.gravity = m_gravity; + i_param.use_filter = m_use_filter_bool; + if(m_filters.size() > 0) { + int dim; + std::vector A; + std::vector B; + m_filters[0]->getParameter(dim, A, B); + i_param.filter_param.length(2*(dim+1)); + for(int i = 0; i < dim+1; i++) { + i_param.filter_param[i] = B[i]; + i_param.filter_param[i + dim + 1] = A[i]; + } + } + return true; +} + +extern "C" +{ + void AccelerationFilterInit(RTC::Manager* manager) + { + coil::Properties profile(accelerationfilter_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } +}; + + + diff --git a/rtc/AccelerationFilter/AccelerationFilter.h b/rtc/AccelerationFilter/AccelerationFilter.h new file mode 100644 index 00000000000..04de90a5a42 --- /dev/null +++ b/rtc/AccelerationFilter/AccelerationFilter.h @@ -0,0 +1,159 @@ +// -*- C++ -*- +/*! + * @file AccelerationFilter.h * @brief Acceleration Filter component * @date $Date$ + * + * $Id$ + */ +#ifndef ACCELERATIONFILTER_H +#define ACCELERATIONFILTER_H + +#include +#include +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/AccelerationFilterService.hh" +#include +#include +#include +#include +#include +#include +#include +// +#include +// +#include <../TorqueFilter/IIRFilter.h> + +// Service implementation headers +// +#include "AccelerationFilterService_impl.h" + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +class AccelerationFilter : public RTC::DataFlowComponentBase +{ +public: + AccelerationFilter(RTC::Manager* manager); + ~AccelerationFilter(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + bool resetFilter(const OpenHRP::AccelerationFilterService::ControlMode &mode, + const double *vel); + bool setParam(const ::OpenHRP::AccelerationFilterService::AccelerationFilterParam& i_param); + bool getParam(::OpenHRP::AccelerationFilterService::AccelerationFilterParam &i_param); + +protected: + // Configuration variable declaration + // + // + + // DataInPort declaration + // + TimedAcceleration3D m_accIn; + InPort m_accInIn; + TimedAngularVelocity3D m_rateIn; + InPort m_rateInIn; + TimedOrientation3D m_rpyIn; + InPort m_rpyInIn; + TimedPoint3D m_posIn; + InPort m_posInIn; + // + + // DataOutPort declaration + // + TimedVector3D m_velOut; + OutPort m_velOutOut; + //TimedPoint3D m_posOut; + //OutPort m_posOutOut; + + // + + // CORBA Port declaration + // + RTC::CorbaPort m_AccelerationFilterServicePort; + + // + + // Service declaration + // + AccelerationFilterService_impl m_service0; + + // + + // Consumer declaration + // + + // + +private: + typedef boost::shared_ptr< IIRFilter> IIRFilterPtr; + double m_dt; + double m_gravity; + bool m_use_filter_bool; + hrp::Vector3 m_global_vel; + std::vector m_filters; + hrp::Vector3 m_previous_pos; + + coil::Mutex m_mutex; +}; + + +extern "C" +{ + DLL_EXPORT void AccelerationFilterInit(RTC::Manager* manager); +}; + +#endif // ACCELERATIONFILTER_H + diff --git a/rtc/AccelerationFilter/AccelerationFilterComp.cpp b/rtc/AccelerationFilter/AccelerationFilterComp.cpp new file mode 100644 index 00000000000..9436137b084 --- /dev/null +++ b/rtc/AccelerationFilter/AccelerationFilterComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file AccelerationFilterComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ +#include +#include +#include +#include "AccelerationFilter.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + AccelerationFilterInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("AccelerationFilter"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + // RTC::RTObject_var rtobj; + // rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + // PortServiceList* portlist; + // portlist = rtobj->get_ports(); + + // getting port profiles + // std::cout << "Number of Ports: "; + // std::cout << portlist->length() << std::endl << std::endl; + // for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + // { + // Port_ptr port; + // port = (*portlist)[i]; + // std::cout << "Port" << i << " (name): "; + // std::cout << port->get_port_profile()->name << std::endl; + // + // RTC::PortInterfaceProfileList iflist; + // iflist = port->get_port_profile()->interfaces; + // std::cout << "---interfaces---" << std::endl; + // for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + // { + // std::cout << "I/F name: "; + // std::cout << iflist[i].instance_name << std::endl; + // std::cout << "I/F type: "; + // std::cout << iflist[i].type_name << std::endl; + // const char* pol; + // pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + // std::cout << "Polarity: " << pol << std::endl; + // } + // std::cout << "---properties---" << std::endl; + // NVUtil::dump(port->get_port_profile()->properties); + // std::cout << "----------------" << std::endl << std::endl; + // } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} + diff --git a/rtc/AccelerationFilter/AccelerationFilterService_impl.cpp b/rtc/AccelerationFilter/AccelerationFilterService_impl.cpp new file mode 100644 index 00000000000..d5bb56b4fd8 --- /dev/null +++ b/rtc/AccelerationFilter/AccelerationFilterService_impl.cpp @@ -0,0 +1,57 @@ +// -*-C++-*- +/*! + * @file ../AccelerationFilterService_impl.cpp + * @brief Service implementation code of ../AccelerationFilterService.idl + * + */ +#include +#include "AccelerationFilterService_impl.h" + +#include "AccelerationFilter.h" + +/* + * Example implementational code for IDL interface OpenHRP::AccelerationFilterService + */ +AccelerationFilterService_impl::AccelerationFilterService_impl() +{ + // Please add extra constructor code here. +} + + +AccelerationFilterService_impl::~AccelerationFilterService_impl() +{ + // Please add extra destructor code here. +} + + +/* + * Methods corresponding to IDL attributes and operations + */ +::CORBA::Boolean AccelerationFilterService_impl::setAccelerationFilterParam( + const ::OpenHRP::AccelerationFilterService::AccelerationFilterParam& i_param) +{ + return m_instance->setParam(i_param); +} + +::CORBA::Boolean AccelerationFilterService_impl::getAccelerationFilterParam( + OpenHRP::AccelerationFilterService::AccelerationFilterParam_out i_param) +{ + i_param = new OpenHRP::AccelerationFilterService::AccelerationFilterParam(); + i_param->filter_param.length(0); + return m_instance->getParam(*i_param); +} + +::CORBA::Boolean AccelerationFilterService_impl::resetFilter(OpenHRP::AccelerationFilterService::ControlMode mode, + const ::OpenHRP::AccelerationFilterService::DblArray3 vel) +{ + // Please insert your code here and remove the following warning pragma + return m_instance->resetFilter(mode, vel); +} + +// End of example implementational code + +void AccelerationFilterService_impl::setInstance(AccelerationFilter *i_instance) +{ + m_instance = i_instance; +} + diff --git a/rtc/AccelerationFilter/AccelerationFilterService_impl.h b/rtc/AccelerationFilter/AccelerationFilterService_impl.h new file mode 100644 index 00000000000..9ab47d27c2c --- /dev/null +++ b/rtc/AccelerationFilter/AccelerationFilterService_impl.h @@ -0,0 +1,48 @@ +// -*-C++-*- +/*! + * @file ../AccelerationFilterService_impl.h + * @brief Service implementation header of ../AccelerationFilterService.idl + * + */ + +#include "hrpsys/idl/AccelerationFilterService.hh" + + +#ifndef ACCELERATIONFILTERSERVICE_IMPL_H +#define ACCELERATIONFILTERSERVICE_IMPL_H + +class AccelerationFilter; + + +/* + * Example class implementing IDL interface OpenHRP::AccelerationFilterService + */ +class AccelerationFilterService_impl + : public virtual POA_OpenHRP::AccelerationFilterService, + public virtual PortableServer::RefCountServantBase +{ +private: + // Make sure all instances are built on the heap by making the + // destructor non-public + //virtual ~OpenHRP_AccelerationFilterService_impl(); + AccelerationFilter *m_instance; + +public: + // standard constructor + AccelerationFilterService_impl(); + virtual ~AccelerationFilterService_impl(); + + // attributes and operations + ::CORBA::Boolean setAccelerationFilterParam(const ::OpenHRP::AccelerationFilterService::AccelerationFilterParam& i_param); + ::CORBA::Boolean getAccelerationFilterParam(::OpenHRP::AccelerationFilterService::AccelerationFilterParam_out i_param); + ::CORBA::Boolean resetFilter(OpenHRP::AccelerationFilterService::ControlMode mode, + const ::OpenHRP::AccelerationFilterService::DblArray3 vel); + // + void setInstance(AccelerationFilter *i_instance); +}; + + + +#endif // ACCELERATIONFILTERSERVICE_IMPL_H + + diff --git a/rtc/AccelerationFilter/CMakeLists.txt b/rtc/AccelerationFilter/CMakeLists.txt new file mode 100644 index 00000000000..3ac5d1f8fb0 --- /dev/null +++ b/rtc/AccelerationFilter/CMakeLists.txt @@ -0,0 +1,18 @@ +set(comp_sources AccelerationFilter.cpp AccelerationFilterService_impl.cpp ../TorqueFilter/IIRFilter.cpp) + +set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +add_library(AccelerationFilter SHARED ${comp_sources}) +target_link_libraries(AccelerationFilter ${libs}) +set_target_properties(AccelerationFilter PROPERTIES PREFIX "") + +add_executable(AccelerationFilterComp AccelerationFilterComp.cpp ${comp_sources}) +target_link_libraries(AccelerationFilterComp ${libs}) + +include_directories(${PROJECT_SOURCE_DIR}/rtc/SequencePlayer) + +set(target AccelerationFilter AccelerationFilterComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.cpp b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.cpp new file mode 100644 index 00000000000..375e9ba1d69 --- /dev/null +++ b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.cpp @@ -0,0 +1,246 @@ +// -*- C++ -*- +/*! + * @file ApproximateVoxelGridFilter.cpp + * @brief Moving Least Squares Filter + * $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "ApproximateVoxelGridFilter.h" +#include "hrpsys/idl/pointcloud.hh" + +// Module specification +// +static const char* spec[] = + { + "implementation_id", "ApproximateVoxelGridFilter", + "type_name", "ApproximateVoxelGridFilter", + "description", "Voxel Grid Filter", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.size", "0.01", + "conf.default.debugLevel", "0", + + "" + }; +// + +ApproximateVoxelGridFilter::ApproximateVoxelGridFilter(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_originalIn("original", m_original), + m_filteredOut("filtered", m_filtered), + // + dummy(0) +{ +} + +ApproximateVoxelGridFilter::~ApproximateVoxelGridFilter() +{ +} + + + +RTC::ReturnCode_t ApproximateVoxelGridFilter::onInitialize() +{ + //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("size", m_size, "0.01"); + bindParameter("debugLevel", m_debugLevel, "0"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("originalIn", m_originalIn); + + // Set OutPort buffer + addOutPort("filteredOut", m_filteredOut); + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + RTC::Properties& prop = getProperties(); + + m_filtered.height = 1; + m_filtered.type = "xyz"; + m_filtered.fields.length(3); + m_filtered.fields[0].name = "x"; + m_filtered.fields[0].offset = 0; + m_filtered.fields[0].data_type = PointCloudTypes::FLOAT32; + m_filtered.fields[0].count = 4; + m_filtered.fields[1].name = "y"; + m_filtered.fields[1].offset = 4; + m_filtered.fields[1].data_type = PointCloudTypes::FLOAT32; + m_filtered.fields[1].count = 4; + m_filtered.fields[2].name = "z"; + m_filtered.fields[2].offset = 8; + m_filtered.fields[2].data_type = PointCloudTypes::FLOAT32; + m_filtered.fields[2].count = 4; + m_filtered.is_bigendian = false; + m_filtered.point_step = 16; + m_filtered.is_dense = true; + + return RTC::RTC_OK; +} + + + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ApproximateVoxelGridFilter::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ApproximateVoxelGridFilter::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ApproximateVoxelGridFilter::onExecute(RTC::UniqueId ec_id) +{ + if (m_debugLevel > 0){ + std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + } + + if (m_originalIn.isNew()){ + m_originalIn.read(); + + if (!m_size){ + m_filtered = m_original; + m_filteredOut.write(); + return RTC::RTC_OK; + } + + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); + + // RTM -> PCL + cloud->points.resize(m_original.width*m_original.height); + float *src = (float *)m_original.data.get_buffer(); + for (unsigned int i=0; ipoints.size(); i++){ + cloud->points[i].x = src[0]; + cloud->points[i].y = src[1]; + cloud->points[i].z = src[2]; + src += 4; + } + + // PCL Processing + pcl::ApproximateVoxelGrid sor; + sor.setInputCloud (cloud); + sor.setLeafSize(m_size, m_size, m_size); + sor.filter(*cloud_filtered); + if (m_debugLevel > 0){ + std::cout << cloud->points.size() << " points are reduced to " + << cloud_filtered->points.size() << " points" << std::endl; + } + + + // PCL -> RTM + m_filtered.width = cloud_filtered->points.size(); + m_filtered.row_step = m_filtered.point_step*m_filtered.width; + m_filtered.data.length(m_filtered.height*m_filtered.row_step); + m_filtered.tm = m_original.tm; + float *dst = (float *)m_filtered.data.get_buffer(); + for (unsigned int i=0; ipoints.size(); i++){ + dst[0] = cloud_filtered->points[i].x; + dst[1] = cloud_filtered->points[i].y; + dst[2] = cloud_filtered->points[i].z; + dst += 4; + } + m_filteredOut.write(); + } + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ApproximateVoxelGridFilter::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + + + +extern "C" +{ + + void ApproximateVoxelGridFilterInit(RTC::Manager* manager) + { + RTC::Properties profile(spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.h b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.h new file mode 100644 index 00000000000..d47b47bedef --- /dev/null +++ b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.h @@ -0,0 +1,148 @@ +// -*- C++ -*- +/*! + * @file ApproximateVoxelGridFilter.h + * @brief Moving Least Squares Filter + * @date $Date$ + * + * $Id$ + */ + +#ifndef VOXEL_GRID_FILTER_H +#define VOXEL_GRID_FILTER_H + +#include +#include "hrpsys/idl/pointcloud.hh" +#include +#include +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief sample RT component which has one data input port and one data output port + */ +class ApproximateVoxelGridFilter + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + ApproximateVoxelGridFilter(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~ApproximateVoxelGridFilter(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + PointCloudTypes::PointCloud m_original; + PointCloudTypes::PointCloud m_filtered; + + // DataInPort declaration + // + InPort m_originalIn; + + // + + // DataOutPort declaration + // + OutPort m_filteredOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + int m_debugLevel; + int dummy; + double m_size; +}; + + +extern "C" +{ + void ApproximateVoxelGridFilterInit(RTC::Manager* manager); +}; + +#endif // VOXEL_GRID_FILTER_H diff --git a/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.txt b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.txt new file mode 100644 index 00000000000..a25f464ada5 --- /dev/null +++ b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilter.txt @@ -0,0 +1,53 @@ +/** + +\page ApproximateVoxelGridFilter + +\section introduction Overview + +This component applies moving least squares filter to an input point cloud. + + + + +
    implementation_idApproximateVoxelGridFilter
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + +
    port namedata typeunitdescription
    originalPointCloudTypes::PointCloud
    + +\subsection outports Output Ports + + + + +
    port namedata typeunitdescription
    filteredPointCloudTypes::PointCloud
    + +\section serviceports Service Ports + +\subsection provider Service Providers + + + +
    port nameinterface nameservice typeIDLdescription
    + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + + +
    nametypeunitdefault valuedescription
    sizedouble[m]0.01size of voxel grid
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilterComp.cpp b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilterComp.cpp new file mode 100644 index 00000000000..acbec2c9740 --- /dev/null +++ b/rtc/ApproximateVoxelGridFilter/ApproximateVoxelGridFilterComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file ApproximateVoxelGridFilterComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "ApproximateVoxelGridFilter.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + ApproximateVoxelGridFilterInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("ApproximateVoxelGridFilter"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/ApproximateVoxelGridFilter/CMakeLists.txt b/rtc/ApproximateVoxelGridFilter/CMakeLists.txt new file mode 100644 index 00000000000..1baa8e095ec --- /dev/null +++ b/rtc/ApproximateVoxelGridFilter/CMakeLists.txt @@ -0,0 +1,19 @@ +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +set(comp_sources ApproximateVoxelGridFilter.cpp) +set(libs hrpsysBaseStub ${PCL_LIBRARIES}) +add_library(ApproximateVoxelGridFilter SHARED ${comp_sources}) +target_link_libraries(ApproximateVoxelGridFilter ${libs}) +set_target_properties(ApproximateVoxelGridFilter PROPERTIES PREFIX "") + +add_executable(ApproximateVoxelGridFilterComp ApproximateVoxelGridFilterComp.cpp ${comp_sources}) +target_link_libraries(ApproximateVoxelGridFilterComp ${libs}) + +set(target ApproximateVoxelGridFilter ApproximateVoxelGridFilterComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index d1882aa987e..01d764a937f 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -14,7 +14,7 @@ #include "AutoBalancer.h" #include #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" typedef coil::Guard Guard; @@ -40,31 +40,79 @@ static const char* autobalancer_spec[] = }; //     +static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm) +{ + int pre = os.precision(); + os.setf(std::ios::fixed); + os << std::setprecision(6) + << (tm.sec + tm.nsec/1e9) + << std::setprecision(pre); + os.unsetf(std::ios::fixed); + return os; +} + AutoBalancer::AutoBalancer(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // m_qRefIn("qRef", m_qRef), + m_qCurrentIn("qCurrent", m_qCurrent), + m_qTouchWallIn("qTouchWall", m_qTouchWall), m_basePosIn("basePosIn", m_basePos), m_baseRpyIn("baseRpyIn", m_baseRpy), m_zmpIn("zmpIn", m_zmp), m_optionalDataIn("optionalData", m_optionalData), - m_emergencySignalIn("emergencySignal", m_emergencySignal), + m_emergencySignalOut("emergencySignal", m_emergencySignal), + m_emergencyFallMotionOut("emergencyFallMotion", m_emergencyFallMotion), + m_isStuckOut("isStuck", m_isStuck), + m_useFlywheelOut("useFlywheel", m_useFlywheel), + m_estimatedFxyOut("estimatedFxy", m_estimatedFxy), + m_diffCPIn("diffCapturePoint", m_diffCP), + m_refFootOriginExtMomentIn("refFootOriginExtMoment", m_refFootOriginExtMoment), + m_refFootOriginExtMomentIsHoldValueIn("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue), + m_touchWallMotionSolvedIn("touchWallMotionSolved", m_touchWallMotionSolved), + m_rpyIn("rpy", m_rpy), + m_qRefSeqIn("qRefSeq", m_qRefSeq), + m_landingHeightIn("landingHeight", m_landingHeight), + m_steppableRegionIn("steppableRegion", m_steppableRegion), m_qOut("q", m_qRef), + m_qAbcOut("qAbc", m_qAbc), + m_tauOut("tau", m_tau), m_zmpOut("zmpOut", m_zmp), + m_zmpActOut("zmpAct", m_zmpAct), + m_refCPOut("refCapturePoint", m_refCP), + m_actCPOut("actCapturePoint", m_actCP), + m_actContactStatesOut("actContactStates", m_actContactStates), + m_COPInfoOut("COPInfo", m_COPInfo), m_basePosOut("basePosOut", m_basePos), m_baseRpyOut("baseRpyOut", m_baseRpy), m_baseTformOut("baseTformOut", m_baseTform), + m_tmpOut("tmp", m_tmp), + m_currentLandingPosOut("currentLandingPos", m_currentLandingPos), + m_diffFootOriginExtMomentOut("diffFootOriginExtMoment", m_diffFootOriginExtMoment), m_basePoseOut("basePoseOut", m_basePose), m_accRefOut("accRef", m_accRef), m_contactStatesOut("contactStates", m_contactStates), + m_toeheelRatioOut("toeheelRatio", m_toeheelRatio), m_controlSwingSupportTimeOut("controlSwingSupportTime", m_controlSwingSupportTime), - m_cogOut("cogOut", m_cog), - m_AutoBalancerServicePort("AutoBalancerService"), m_walkingStatesOut("walkingStates", m_walkingStates), m_sbpCogOffsetOut("sbpCogOffset", m_sbpCogOffset), + m_cogOut("cogOut", m_cog), + m_allEECompOut("allEEComp", m_allEEComp), + m_landingTargetOut("landingTarget", m_landingTarget), + m_endCogStateOut("endCogState", m_endCogState), + m_AutoBalancerServicePort("AutoBalancerService"), + m_RobotHardwareServicePort("RobotHardwareService"), + // for debug output + m_originRefZmpOut("originRefZmp", m_originRefZmp), + m_originRefCogOut("originRefCog", m_originRefCog), + m_originRefCogVelOut("originRefCogVel", m_originRefCogVel), + m_originNewZmpOut("originNewZmp", m_originNewZmp), + m_originActZmpOut("originActZmp", m_originActZmp), + m_originActCogOut("originActCog", m_originActCog), + m_originActCogVelOut("originActCogVel", m_originActCogVel), + m_currentSteppableRegionOut("currentSteppableRegion", m_currentSteppableRegion), // gait_type(BIPED), - move_base_gain(0.8), m_robot(hrp::BodyPtr()), m_debugLevel(0) { @@ -85,38 +133,73 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() // // Set InPort buffers addInPort("qRef", m_qRefIn); + addInPort("qCurrent", m_qCurrentIn); + addInPort("qTouchWall", m_qTouchWallIn); addInPort("basePosIn", m_basePosIn); addInPort("baseRpyIn", m_baseRpyIn); addInPort("zmpIn", m_zmpIn); addInPort("optionalData", m_optionalDataIn); - addInPort("emergencySignal", m_emergencySignalIn); + addInPort("diffCapturePoint", m_diffCPIn); + addInPort("refFootOriginExtMoment", m_refFootOriginExtMomentIn); + addInPort("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValueIn); + addInPort("touchWallMotionSolved", m_touchWallMotionSolvedIn); + addInPort("rpy", m_rpyIn); + addInPort("qRefSeq", m_qRefSeqIn); + addInPort("landingHeight", m_landingHeightIn); + addInPort("steppableRegion", m_steppableRegionIn); // Set OutPort buffer addOutPort("q", m_qOut); + addOutPort("qAbc", m_qAbcOut); + addOutPort("tau", m_tauOut); addOutPort("zmpOut", m_zmpOut); + addOutPort("zmpAct", m_zmpActOut); + addOutPort("refCapturePoint", m_refCPOut); + addOutPort("actCapturePoint", m_actCPOut); + addOutPort("actContactStates", m_actContactStatesOut); + addOutPort("COPInfo", m_COPInfoOut); addOutPort("basePosOut", m_basePosOut); addOutPort("baseRpyOut", m_baseRpyOut); addOutPort("baseTformOut", m_baseTformOut); + addOutPort("tmpOut", m_tmpOut); + addOutPort("currentLandingPosOut", m_currentLandingPosOut); + addOutPort("diffFootOriginExtMoment", m_diffFootOriginExtMomentOut); + addOutPort("allEEComp", m_allEECompOut); addOutPort("basePoseOut", m_basePoseOut); addOutPort("accRef", m_accRefOut); addOutPort("contactStates", m_contactStatesOut); + addOutPort("toeheelRatio", m_toeheelRatioOut); addOutPort("controlSwingSupportTime", m_controlSwingSupportTimeOut); addOutPort("cogOut", m_cogOut); addOutPort("walkingStates", m_walkingStatesOut); addOutPort("sbpCogOffset", m_sbpCogOffsetOut); - + addOutPort("emergencySignal", m_emergencySignalOut); + addOutPort("emergencyFallMotion", m_emergencyFallMotionOut); + addOutPort("isStuck", m_isStuckOut); + addOutPort("useFlywheel", m_useFlywheelOut); + addOutPort("estimatedFxy", m_estimatedFxyOut); + addOutPort("landingTarget", m_landingTargetOut); + addOutPort("endCogState", m_endCogStateOut); + // for debug output + addOutPort("originRefZmp", m_originRefZmpOut); + addOutPort("originRefCog", m_originRefCogOut); + addOutPort("originRefCogVel", m_originRefCogVelOut); + addOutPort("originNewZmp", m_originNewZmpOut); + addOutPort("originActZmp", m_originActZmpOut); + addOutPort("originActCog", m_originActCogOut); + addOutPort("originActCogVel", m_originActCogVelOut); + addOutPort("currentSteppableRegion", m_currentSteppableRegionOut); + // Set service provider to Ports m_AutoBalancerServicePort.registerProvider("service0", "AutoBalancerService", m_service0); - - // Set service consumers to Ports - + // Set CORBA Service Ports addPort(m_AutoBalancerServicePort); - + // // // Bind variables and configuration variable - + // RTC::Properties& prop = getProperties(); @@ -131,7 +214,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() } nameServer = nameServer.substr(0, comPos); RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); - if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){ std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl; @@ -140,9 +223,24 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() // allocate memory for outPorts m_qRef.data.length(m_robot->numJoints()); - qorg.resize(m_robot->numJoints()); - qrefv.resize(m_robot->numJoints()); + m_qAbc.data.length(m_robot->numJoints()); + m_qCurrent.data.length(m_robot->numJoints()); + m_qRefSeq.data.length(m_robot->numJoints()); + m_tau.data.length(m_robot->numJoints()); + m_qTouchWall.data.length(m_robot->numJoints()); m_baseTform.data.length(12); + m_tmp.data.length(33); + diff_q.resize(m_robot->numJoints()); + // for debug output + m_originRefZmp.data.x = m_originRefZmp.data.y = m_originRefZmp.data.z = 0.0; + m_originRefCog.data.x = m_originRefCog.data.y = m_originRefCog.data.z = 0.0; + m_originRefCogVel.data.x = m_originRefCogVel.data.y = m_originRefCogVel.data.z = 0.0; + m_originNewZmp.data.x = m_originNewZmp.data.y = m_originNewZmp.data.z = 0.0; + m_originActZmp.data.x = m_originActZmp.data.y = m_originActZmp.data.z = 0.0; + m_originActCog.data.x = m_originActCog.data.y = m_originActCog.data.z = 0.0; + m_originActCogVel.data.x = m_originActCogVel.data.y = m_originActCogVel.data.z = 0.0; + m_currentSteppableRegion.data.region.length(1); + m_currentSteppableRegion.data.region[0].length(1); control_mode = MODE_IDLE; loop = 0; @@ -154,6 +252,18 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() leg_names.push_back("rleg"); leg_names.push_back("lleg"); + // Generate FIK + + fik = fikPtr(new FullbodyInverseKinematicsSolver(m_robot, std::string(m_profile.instance_name), m_dt)); + ik_mode = OpenHRP::AutoBalancerService::SIMPLE; + // Generate ST + st = stPtr(new Stabilizer(m_robot, std::string(m_profile.instance_name), m_dt)); + + // Set service consumers to Ports + m_RobotHardwareServicePort.registerConsumer("service0", "RobotHardwareService", st->m_robotHardwareService0); + // Set CORBA Service Ports + addPort(m_RobotHardwareServicePort); + // setting from conf file // rleg,TARGET_LINK,BASE_LINK coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ","); @@ -166,6 +276,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str()); coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str()); ABCIKparam tp; + hrp::Link* root = m_robot->link(ee_target); for (size_t j = 0; j < 3; j++) { coil::stringTo(tp.localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); } @@ -174,42 +285,125 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str()); } tp.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle - tp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name))); + // FIK param + FullbodyInverseKinematicsSolver::IKparam tmp_fikp; + tmp_fikp.manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name))); + tmp_fikp.target_link = m_robot->link(ee_target); + tmp_fikp.localPos = tp.localPos; + tmp_fikp.localR = tp.localR; + tmp_fikp.max_limb_length = tp.localPos.norm(); + while (!root->isRoot()) { + tmp_fikp.max_limb_length += root->b.norm(); + tmp_fikp.parent_name = root->name; + tmp_fikp.group_indices.push_back(root->jointId); + root = root->parent; + } + fik->ikp.insert(std::pair(ee_name, tmp_fikp)); + // ST param + Stabilizer::STIKParam stp; + for (size_t j = 0; j < 3; j++) { + stp.localp(j) = tp.localPos(j); + } + stp.localR = tp.localR; + stp.target_name = ee_target; + stp.ee_name = ee_name; + stp.avoid_gain = 0.001; + stp.reference_gain = 0.01; + stp.ik_loop_count = 3; + // For swing ee modification + stp.target_ee_diff_p = hrp::Vector3::Zero(); + stp.d_rpy_swing = hrp::Vector3::Zero(); + stp.d_pos_swing = hrp::Vector3::Zero(); + stp.target_ee_diff_p_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(50.0, m_dt, hrp::Vector3::Zero())); // [Hz] + stp.prev_d_pos_swing = hrp::Vector3::Zero(); + stp.prev_d_rpy_swing = hrp::Vector3::Zero(); + stp.contact_phase = Stabilizer::SUPPORT_PHASE; + stp.phase_time = 0; + { + bool is_ee_exists = false; + for (size_t j = 0; j < m_robot->numSensors(hrp::Sensor::FORCE); j++) { + hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, j); + hrp::Link* alink = m_robot->link(stp.target_name); + while (alink != NULL && alink->name != ee_base && !is_ee_exists) { + if ( alink->name == sensor->link->name ) { + is_ee_exists = true; + stp.sensor_name = sensor->name; + } + alink = alink->parent; + } + } + } + st->stikp.push_back(stp); + st->jpe_v.push_back(hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), m_dt, false, std::string(m_profile.instance_name)))); + // Fix for toe joint + if (ee_name.find("leg") != std::string::npos && st->jpe_v.back()->numJoints() == 7) { // leg and has 7dof joint (6dof leg +1dof toe) + std::vector optw; + for (int j = 0; j < st->jpe_v.back()->numJoints(); j++ ) { + if ( j == st->jpe_v.back()->numJoints()-1 ) optw.push_back(0.0); + else optw.push_back(1.0); + } + st->jpe_v.back()->setOptionalWeightVector(optw); + } + for (int j = 0; j < st->jpe_v.back()->numJoints(); j++ ) { + st->stikp.back().support_pgain = hrp::dvector::Constant(st->jpe_v.back()->numJoints(),100); + st->stikp.back().support_dgain = hrp::dvector::Constant(st->jpe_v.back()->numJoints(),100); + st->stikp.back().landing_pgain = hrp::dvector::Constant(st->jpe_v.back()->numJoints(),100); + st->stikp.back().landing_dgain = hrp::dvector::Constant(st->jpe_v.back()->numJoints(),100); + st->stikp.back().swing_pgain = hrp::dvector::Constant(st->jpe_v.back()->numJoints(),100); + st->stikp.back().swing_dgain = hrp::dvector::Constant(st->jpe_v.back()->numJoints(),100); + } + st->contact_states_index_map.insert(std::pair(ee_name, i)); + st->is_ik_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands ik => disabled, feet ik => enabled, by default + st->is_feedback_control_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands feedback control => disabled, feet feedback control => enabled, by default + st->is_zmp_calc_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // To zmp calculation, hands are disabled and feet are enabled, by default // Fix for toe joint // Toe joint is defined as end-link joint in the case that end-effector link != force-sensor link // Without toe joints, "end-effector link == force-sensor link" is assumed. // With toe joints, "end-effector link != force-sensor link" is assumed. if (m_robot->link(ee_target)->sensors.size() == 0) { // If end-effector link has no force sensor - std::vector optw(tp.manip->numJoints(), 1.0); + std::vector optw(fik->ikp[ee_name].manip->numJoints(), 1.0); optw.back() = 0.0; // Set weight = 0 for toe joint by default - tp.manip->setOptionalWeightVector(optw); + fik->ikp[ee_name].manip->setOptionalWeightVector(optw); tp.has_toe_joint = true; } else { tp.has_toe_joint = false; } - tp.avoid_gain = 0.001; - tp.reference_gain = 0.01; - tp.pos_ik_error_count = tp.rot_ik_error_count = 0; + tp.target_link = m_robot->link(ee_target); ikp.insert(std::pair(ee_name , tp)); - ikp[ee_name].target_link = m_robot->link(ee_target); ee_vec.push_back(ee_name); std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] target = " << ikp[ee_name].target_link->name << ", base = " << ee_base << std::endl; std::cerr << "[" << m_profile.instance_name << "] offset_pos = " << tp.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] has_toe_joint = " << (tp.has_toe_joint?"true":"false") << std::endl; contact_states_index_map.insert(std::pair(ee_name, i)); + if (( ee_name == "lleg" ) || ( ee_name == "rleg" )) touchdown_transition_interpolator.insert(std::pair(ee_name, new interpolator(1, m_dt, interpolator::CUBICSPLINE))); + st->swing_modification_interpolator.insert(std::pair(ee_name, new interpolator(1, m_dt, interpolator::CUBICSPLINE))); } m_contactStates.data.length(num); + m_actContactStates.data.length(num); + m_toeheelRatio.data.length(num); if (ikp.find("rleg") != ikp.end() && ikp.find("lleg") != ikp.end()) { m_contactStates.data[contact_states_index_map["rleg"]] = true; m_contactStates.data[contact_states_index_map["lleg"]] = true; + m_actContactStates.data[contact_states_index_map["rleg"]] = true; + m_actContactStates.data[contact_states_index_map["lleg"]] = true; } if (ikp.find("rarm") != ikp.end() && ikp.find("larm") != ikp.end()) { m_contactStates.data[contact_states_index_map["rarm"]] = false; m_contactStates.data[contact_states_index_map["larm"]] = false; + m_actContactStates.data[contact_states_index_map["rarm"]] = false; + m_actContactStates.data[contact_states_index_map["larm"]] = false; } m_controlSwingSupportTime.data.length(num); - for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 0.0; + for (size_t i = 0; i < num; i++) m_controlSwingSupportTime.data[i] = 1.0; + for (size_t i = 0; i < num; i++) m_toeheelRatio.data[i] = rats::no_using_toe_heel_ratio; + m_COPInfo.data.length(m_contactStates.data.length()*3); // nx, ny, fz for each end-effectors + for (size_t i = 0; i < m_COPInfo.data.length(); i++) { + m_COPInfo.data[i] = 0.0; + } + + // ST param + st->initStabilizer(prop, num); } std::vector leg_pos; if (leg_offset_str.size() > 0) { @@ -229,10 +423,7 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() std::vector > interlocking_joints; readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(m_profile.instance_name)); if (interlocking_joints.size() > 0) { - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - std::cerr << "[" << m_profile.instance_name << "] Interlocking Joints for [" << it->first << "]" << std::endl; - it->second.manip->setInterlockingJointPairIndices(interlocking_joints, std::string(m_profile.instance_name)); - } + fik->initializeInterlockingJoints(interlocking_joints); } zmp_offset_interpolator = new interpolator(ikp.size()*3, m_dt); @@ -240,7 +431,11 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() zmp_transition_time = 1.0; transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); transition_interpolator->setName(std::string(m_profile.instance_name)+" transition_interpolator"); - transition_interpolator_ratio = 1.0; + transition_interpolator_ratio = 0.0; + emergency_transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + emergency_transition_interpolator->setName(std::string(m_profile.instance_name)+" emergency_transition_interpolator"); + touch_wall_transition_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); + touch_wall_transition_interpolator->setName(std::string(m_profile.instance_name)+" touch_wall_transition_interpolator"); adjust_footstep_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); adjust_footstep_interpolator->setName(std::string(m_profile.instance_name)+" adjust_footstep_interpolator"); transition_time = 2.0; @@ -248,18 +443,39 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() leg_names_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB, 1); leg_names_interpolator->setName(std::string(m_profile.instance_name)+" leg_names_interpolator"); leg_names_interpolator_ratio = 1.0; + angular_momentum_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB); + angular_momentum_interpolator->setName(std::string(m_profile.instance_name)+" angular_momentum_interpolator"); + roll_weight_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB); + roll_weight_interpolator->setName(std::string(m_profile.instance_name)+" roll_weight_interpolator"); + pitch_weight_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB); + pitch_weight_interpolator->setName(std::string(m_profile.instance_name)+" pitch_weight_interpolator"); + roll_momentum_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB); + roll_momentum_interpolator->setName(std::string(m_profile.instance_name)+" roll_momentum_interpolator"); + pitch_momentum_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB); + pitch_momentum_interpolator->setName(std::string(m_profile.instance_name)+" pitch_momentum_interpolator"); + go_vel_interpolator = new interpolator(3, m_dt, interpolator::HOFFARBIB); + go_vel_interpolator->setName(std::string(m_profile.instance_name)+" go_vel_interpolator"); + cog_constraint_interpolator = new interpolator(1, m_dt, interpolator::HOFFARBIB); + cog_constraint_interpolator->setName(std::string(m_profile.instance_name)+" cog_constraint_interpolator"); + limit_cog_interpolator = new interpolator(3, m_dt, interpolator::CUBICSPLINE); + limit_cog_interpolator->setName(std::string(m_profile.instance_name)+" limit_cog_interpolator"); + hand_fix_interpolator = new interpolator(3, m_dt, interpolator::CUBICSPLINE); + hand_fix_interpolator->setName(std::string(m_profile.instance_name)+" hand_fix_interpolator"); // setting stride limitations from conf file double stride_fwd_x_limit = 0.15; - double stride_y_limit = 0.05; - double stride_th_limit = 10; + double stride_outside_y_limit = 0.05; + double stride_outside_th_limit = 10; double stride_bwd_x_limit = 0.05; - std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_y_limit << "[m], " << stride_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl; + double stride_inside_y_limit = stride_outside_y_limit*0.5; + double stride_inside_th_limit = stride_outside_th_limit*0.5; + std::cerr << "[" << m_profile.instance_name << "] abc_stride_parameter : " << stride_fwd_x_limit << "[m], " << stride_outside_y_limit << "[m], " << stride_outside_th_limit << "[deg], " << stride_bwd_x_limit << "[m]" << std::endl; if (default_zmp_offsets.size() == 0) { for (size_t i = 0; i < ikp.size(); i++) default_zmp_offsets.push_back(hrp::Vector3::Zero()); } if (leg_offset_str.size() > 0) { - gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_y_limit/*[m]*/, stride_th_limit/*[deg]*/, stride_bwd_x_limit/*[m]*/)); + gg = ggPtr(new rats::gait_generator(m_dt, leg_pos, leg_names, stride_fwd_x_limit/*[m]*/, stride_outside_y_limit/*[m]*/, stride_outside_th_limit/*[deg]*/, + stride_bwd_x_limit/*[m]*/, stride_inside_y_limit/*[m]*/, stride_inside_th_limit/*[m]*/)); gg->set_default_zmp_offsets(default_zmp_offsets); } gg_is_walking = gg_solved = false; @@ -269,21 +485,32 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() // load virtual force sensors readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name)); // ref force port - int npforce = m_robot->numSensors(hrp::Sensor::FORCE); - int nvforce = m_vfs.size(); - int nforce = npforce + nvforce; + unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE); + unsigned int nvforce = m_vfs.size(); + unsigned int nforce = npforce + nvforce; + // check number of force sensors + if (nforce < m_contactStates.data.length()) { + std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has less force sensors(" << nforce; + std::cerr << ") than end-effector settings(" << m_contactStates.data.length() << ") !" << std::endl; + } + m_ref_force.resize(nforce); m_ref_forceIn.resize(nforce); m_force.resize(nforce); m_ref_forceOut.resize(nforce); m_limbCOPOffset.resize(nforce); m_limbCOPOffsetOut.resize(nforce); + m_wrenches.resize(nforce); + m_wrenchesIn.resize(nforce); + st->wrenches.resize(nforce); + st->ref_wrenches.resize(nforce); + st->limbCOPOffset.resize(nforce); for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); } for (unsigned int i=0; i::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) { - if (it->second.id == i) sensor_names.push_back(it->first); + if (it->second.id == (int)i) sensor_names.push_back(it->first); } } // set ref force port @@ -294,6 +521,14 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() registerInPort(std::string("ref_"+sensor_names[i]).c_str(), *m_ref_forceIn[i]); std::cerr << "[" << m_profile.instance_name << "] name = " << std::string("ref_"+sensor_names[i]) << std::endl; ref_forces.push_back(hrp::Vector3(0,0,0)); + ref_moments.push_back(hrp::Vector3(0,0,0)); + // actual inport + m_wrenchesIn[i] = new InPort(sensor_names[i].c_str(), m_wrenches[i]); + m_wrenches[i].data.length(6); + m_allEEComp.data.length(st->stikp.size() * 6); // 6 is pos+rot dim + st->wrenches[i].resize(6); + st->ref_wrenches[i].resize(6); + registerInPort(sensor_names[i].c_str(), *m_wrenchesIn[i]); } // set force port for (unsigned int i=0; i(0.2/m_dt); // once per 0.2 [s] + use_act_states = false; + gg->use_act_states = st->use_act_states = true; + + is_after_walking = false; + prev_roll_state = prev_pitch_state = false; + prev_orig_cog = orig_cog = hrp::Vector3::Zero(); + prev_orig_dif_p = orig_dif_p = hrp::Vector3::Zero(); + + is_emergency_step_mode = false; + is_emergency_touch_wall_mode = false; + is_emergency_stopping = false; + is_touch_wall_motion_solved = false; + touch_wall_retrieve_time = 0.5; + use_collision_avoidance = false; + is_natural_walk = false; + is_stop_early_foot = false; + was_exceed_q_ref_err_thre = false; + + cog_z_constraint = 1; + arm_swing_deg = 30.0; + ik_max_loop = 3; + + debug_read_steppable_region = false; + + // TODO: use model/idl + wheel_radius = 0.04; + wheel_id["rleg"] = 37; + wheel_id["lleg"] = 38; + wheel_id["rleg_rear"] = 39; + wheel_id["lleg_rear"] = 40; + prev_wheel_pos["rleg"] = 0.0; + prev_wheel_pos["lleg"] = 0.0; + d_wheel_angle = 0.0; + start_d_wheel_angle = 0.0; + hrp::Sensor* sen = m_robot->sensor("gyrometer"); + if (sen == NULL) { + std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl; + } + + additional_force_applied_link = m_robot->rootLink(); + additional_force_applied_point_offset = hrp::Vector3::Zero(); return RTC::RTC_OK; } @@ -348,8 +620,31 @@ RTC::ReturnCode_t AutoBalancer::onFinalize() { delete zmp_offset_interpolator; delete transition_interpolator; + delete emergency_transition_interpolator; + delete touch_wall_transition_interpolator; delete adjust_footstep_interpolator; delete leg_names_interpolator; + delete angular_momentum_interpolator; + delete roll_weight_interpolator; + delete pitch_weight_interpolator; + delete roll_momentum_interpolator; + delete pitch_momentum_interpolator; + delete st->transition_interpolator; + delete go_vel_interpolator; + delete cog_constraint_interpolator; + delete limit_cog_interpolator; + delete hand_fix_interpolator; + delete st->after_walking_interpolator; + for ( std::map::iterator it = touchdown_transition_interpolator.begin(); it != touchdown_transition_interpolator.end(); it++ ) { + delete it->second; + } + for ( std::map::iterator it = st->swing_modification_interpolator.begin(); it != st->swing_modification_interpolator.end(); it++ ) { + delete it->second; + } + if (st->szd == NULL) { + delete st->szd; + st->szd = NULL; + } return RTC::RTC_OK; } @@ -370,7 +665,7 @@ RTC::ReturnCode_t AutoBalancer::onFinalize() RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id) { std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; - + return RTC::RTC_OK; } @@ -381,7 +676,12 @@ RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id) if (control_mode == MODE_ABC) { control_mode = MODE_SYNC_TO_IDLE; double tmp_ratio = 0.0; - transition_interpolator->go(&tmp_ratio, m_dt, true); // sync in one controller loop + transition_interpolator->setGoal(&tmp_ratio, m_dt, true); // sync in one controller loop + } + if ( (st->control_mode == Stabilizer::MODE_ST || st->control_mode == Stabilizer::MODE_AIR) ) { + st->sync_2_idle (); + st->control_mode = Stabilizer::MODE_IDLE; + st->transition_count = 1; // sync in one controller loop } return RTC::RTC_OK; } @@ -393,9 +693,30 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) { // std::cerr << "AutoBalancer::onExecute(" << ec_id << ")" << std::endl; loop ++; + + if (is_emergency_step_mode && st->is_emergency_step && !gg_is_walking && !is_stop_mode && gg->is_steppable_while_jumping()) { + if (gg->is_jumping) { + gg->is_jumping = false; + stopJumping(); + } + gg->set_is_emergency_step(true); + // if (fabs(st->act_cp(0)) > fabs(st->act_cp(1))) { + if (st->act_cp(0) > 0) goVelocity(0,(st->ref_foot_origin_rot*ikp["lleg"].target_p0)(0) > (st->ref_foot_origin_rot*ikp["rleg"].target_p0)(0) ? -1e-6:1e-6,0); + else goVelocity(0,(st->ref_foot_origin_rot*ikp["lleg"].target_p0)(0) > (st->ref_foot_origin_rot*ikp["rleg"].target_p0)(0) ? 1e-6:-1e-6,0); + // } else goVelocity(0,st->act_cp(1)>0?-1e-6:1e-6,0); + is_emergency_step_mode = false; + } + + // Read Inport if (m_qRefIn.isNew()) { m_qRefIn.read(); } + if (m_qCurrentIn.isNew()) { + m_qCurrentIn.read(); + } + if (m_qTouchWallIn.isNew()) { + m_qTouchWallIn.read(); + } if (m_basePosIn.isNew()) { m_basePosIn.read(); input_basePos(0) = m_basePos.data.x; @@ -419,59 +740,205 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) } if (m_optionalDataIn.isNew()) { m_optionalDataIn.read(); - if (is_legged_robot) { - if (m_optionalData.data.length() >= contact_states_index_map.size()*2) { - // current optionalData is contactstates x limb and controlSwingSupportTime x limb - // If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false. - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first); - m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()]; - } - if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { // If two feet have no contact, force set double support contact - m_contactStates.data[contact_states_index_map["rleg"]] = true; - m_contactStates.data[contact_states_index_map["lleg"]] = true; - } + } + if (st->reset_emergency_flag) { + m_emergencySignal.data = 0; + m_emergencySignalOut.write(); + m_emergencyFallMotion.data = false; + m_emergencyFallMotionOut.write(); + st->reset_emergency_flag = false; + } else { + if (st->is_emergency_motion && !is_stop_mode) { + std::cerr << "[" << m_profile.instance_name << "] emergencyFallMotion is set!" << std::endl; + is_stop_mode = true; + m_emergencyFallMotion.data = true; + m_emergencyFallMotionOut.write(); + gg->finalize_velocity_mode2(); + stopABCparamEmergency(); + st->stopSTEmergency(); + } + if (gg_is_walking && !is_stop_mode && gg->is_emergency_touch_wall && is_emergency_touch_wall_mode) { + std::cerr << "[" << m_profile.instance_name << "] emergencyTouchWall is set!" << std::endl; + m_emergencySignal.data = 2; + is_stop_mode = true; + m_emergencySignalOut.write(); + double tmp_ratio = 1.0; + touch_wall_transition_interpolator->set(&tmp_ratio); + tmp_ratio = 0.0; + touch_wall_transition_interpolator->setGoal(&tmp_ratio, touch_wall_retrieve_time, true); + gg->emergency_stop(); + is_emergency_stopping = true; + } else if (is_stop_mode && !gg_is_walking && is_emergency_stopping) { + stopABCparamEmergency(); + st->stopSTEmergency(); + is_emergency_stopping = false; + touch_wall_transition_interpolator->clear(); + } + } + if (m_diffCPIn.isNew()) { + m_diffCPIn.read(); + gg->set_diff_cp(hrp::Vector3(m_diffCP.data.x, m_diffCP.data.y, m_diffCP.data.z)); + } + if (m_refFootOriginExtMomentIn.isNew()) { + m_refFootOriginExtMomentIn.read(); + } + if (m_refFootOriginExtMomentIsHoldValueIn.isNew()) { + m_refFootOriginExtMomentIsHoldValueIn.read(); + } + if (m_touchWallMotionSolvedIn.isNew()) { + m_touchWallMotionSolvedIn.read(); + is_touch_wall_motion_solved = m_touchWallMotionSolved.data; + } + if (m_rpyIn.isNew()) { + m_rpyIn.read(); + } + for (size_t i = 0; i < m_wrenchesIn.size(); ++i) { + if ( m_wrenchesIn[i]->isNew() ) { + m_wrenchesIn[i]->read(); + } + } + gg->set_is_vision_updated(false); + if (m_landingHeightIn.isNew()) { + m_landingHeightIn.read(); + if ((gg->get_support_leg_names().front() == "rleg" && m_landingHeight.data.l_r == 0) || + (gg->get_support_leg_names().front() == "lleg" && m_landingHeight.data.l_r == 1)) + { + m_tmp.data[27] = m_landingHeight.data.l_r == 0 ? 1 : 2; + hrp::Vector3 pos(hrp::Vector3(m_landingHeight.data.x, m_landingHeight.data.y, m_landingHeight.data.z)); + hrp::Vector3 normal(hrp::Vector3(m_landingHeight.data.nx, m_landingHeight.data.ny, m_landingHeight.data.nz)); + if (isfinite(pos(2))) { + gg->set_is_vision_updated(true); + gg->set_rel_landing_height(pos); + gg->set_rel_landing_normal(normal); + m_tmp.data[29] = pos(0); + m_tmp.data[30] = pos(1); + m_tmp.data[31] = pos(2); } + } else { + m_tmp.data[27] = m_landingHeight.data.l_r == 0 ? 3 : 4; } + } else { + m_tmp.data[27] = 0; } - if (m_emergencySignalIn.isNew()){ - m_emergencySignalIn.read(); - // if (!is_stop_mode) { - // std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl; - // is_stop_mode = true; - // } + if (m_steppableRegionIn.isNew()) { + m_steppableRegionIn.read(); + if ((gg->get_support_leg_names().front() == "rleg" && m_steppableRegion.data.l_r == 0) || + (gg->get_support_leg_names().front() == "lleg" && m_steppableRegion.data.l_r == 1)) + { + m_tmp.data[28] = m_steppableRegion.data.l_r == 0 ? 1 : 2; + gg->set_steppable_region(m_steppableRegion); + } else { + m_tmp.data[28] = m_steppableRegion.data.l_r == 0 ? 3 : 4; + } + } else { + m_tmp.data[28] = 0; } + // Calculation Guard guard(m_mutex); - hrp::Vector3 ref_basePos; - hrp::Matrix33 ref_baseRot; - hrp::Vector3 rel_ref_zmp; // ref zmp in base frame if ( is_legged_robot ) { - gg->proc_zmp_weight_map_interpolation(); - getCurrentParameters(); + // For parameters + setActData2ST(); + st->getActualParameters(); + fixActCogVelForWheel(); + gg->set_act_contact_states(st->act_contact_states); + calcTotalExternalForceZ(); + // if (!go_vel_interpolator->isEmpty()) { + // std::vector tmp_v(3); + // go_vel_interpolator->get(tmp_v.data(), true); + // gg->set_velocity_param(tmp_v[0], tmp_v[1], tmp_v[2]); + // } getTargetParameters(); + // Get transition ratio bool is_transition_interpolator_empty = transition_interpolator->isEmpty(); if (!is_transition_interpolator_empty) { transition_interpolator->get(&transition_interpolator_ratio, true); } else { - transition_interpolator_ratio = 1.0; + transition_interpolator_ratio = (control_mode == MODE_IDLE) ? 0.0 : 1.0; } if (control_mode != MODE_IDLE ) { - solveLimbIK(); + switch(ik_mode){ + case OpenHRP::AutoBalancerService::SIMPLE: + solveSimpleFullbodyIK(); + break; + case OpenHRP::AutoBalancerService::FULLBODY: + solveFullbodyIK(); + break; + default: + break; + } +// /////// Inverse Dynamics ///////// +// if(!idsb.is_initialized){ +// idsb.setInitState(m_robot, m_dt); +// invdyn_zmp_filters.resize(3); +// for(int i=0;i<3;i++){ +// invdyn_zmp_filters[i].setParameterAsBiquad(25, 1/std::sqrt(2), 1.0/m_dt); +// invdyn_zmp_filters[i].reset(ref_zmp(i)); +// } +// } +// calcAccelerationsForInverseDynamics(m_robot, idsb); +// if(gg_is_walking){ +// calcWorldZMPFromInverseDynamics(m_robot, idsb, ref_zmp); +// for(int i=0;i<3;i++) ref_zmp(i) = invdyn_zmp_filters[i].passFilter(ref_zmp(i)); +// } +// updateInvDynStateBuffer(idsb); + + // overwirte wheel angle + { + if (gg->is_wheeling) d_wheel_angle = start_d_wheel_angle + gg->get_cur_wheel_pos_x() / wheel_radius; + else start_d_wheel_angle = d_wheel_angle; + m_robot->joint(wheel_id["rleg"])->q = m_qRef.data[wheel_id["rleg"]] + d_wheel_angle; + m_robot->joint(wheel_id["lleg"])->q = m_qRef.data[wheel_id["lleg"]] + d_wheel_angle; + m_robot->joint(wheel_id["rleg_rear"])->q = m_qRef.data[wheel_id["rleg_rear"]] + d_wheel_angle; + m_robot->joint(wheel_id["lleg_rear"])->q = m_qRef.data[wheel_id["lleg_rear"]] + d_wheel_angle; + } rel_ref_zmp = m_robot->rootLink()->R.transpose() * (ref_zmp - m_robot->rootLink()->p); + fik->storeCurrentParameters(); + + if (is_touch_wall_motion_solved) { + double tmp_ratio = 1.0; + if (!touch_wall_transition_interpolator->isEmpty()) { + touch_wall_transition_interpolator->get(&tmp_ratio, true); + } else if (is_emergency_stopping) { + tmp_ratio = 0.0; + } + for ( int i = 0; i < m_robot->numJoints(); i++ ) { + m_robot->joint(i)->q = (1-tmp_ratio) * m_qTouchWall.data[i] + tmp_ratio * m_robot->joint(i)->q; + } + } } else { + if (!emergency_transition_interpolator->isEmpty()) { + double tmp_ratio; + emergency_transition_interpolator->get(&tmp_ratio, true); + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = m_qRef.data[i] + tmp_ratio * diff_q[i]; + } + } rel_ref_zmp = input_zmp; + fik->d_root_height = 0.0; + fik->storeCurrentParameters(); } - // transition + // Transition if (!is_transition_interpolator_empty) { // transition_interpolator_ratio 0=>1 : IDLE => ABC // transition_interpolator_ratio 1=>0 : ABC => IDLE ref_basePos = (1-transition_interpolator_ratio) * input_basePos + transition_interpolator_ratio * m_robot->rootLink()->p; rel_ref_zmp = (1-transition_interpolator_ratio) * input_zmp + transition_interpolator_ratio * rel_ref_zmp; rats::mid_rot(ref_baseRot, transition_interpolator_ratio, input_baseRot, m_robot->rootLink()->R); - for ( int i = 0; i < m_robot->numJoints(); i++ ) { + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) { m_robot->joint(i)->q = (1-transition_interpolator_ratio) * m_qRef.data[i] + transition_interpolator_ratio * m_robot->joint(i)->q; } + for (unsigned int i=0; i< m_force.size(); i++) { + for (unsigned int j=0; j<6; j++) { + m_force[i].data[j] = transition_interpolator_ratio * m_force[i].data[j] + (1-transition_interpolator_ratio) * m_ref_force[i].data[j]; + } + } + for (unsigned int i=0; i< m_limbCOPOffset.size(); i++) { + // transition (TODO:set stopABCmode value instead of 0) + m_limbCOPOffset[i].data.x = transition_interpolator_ratio * m_limbCOPOffset[i].data.x;// + (1-transition_interpolator_ratio) * 0; + m_limbCOPOffset[i].data.y = transition_interpolator_ratio * m_limbCOPOffset[i].data.y;// + (1-transition_interpolator_ratio) * 0; + m_limbCOPOffset[i].data.z = transition_interpolator_ratio * m_limbCOPOffset[i].data.z;// + (1-transition_interpolator_ratio) * 0; + } } else { ref_basePos = m_robot->rootLink()->p; ref_baseRot = m_robot->rootLink()->R; @@ -480,17 +947,21 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) if (control_mode == MODE_SYNC_TO_ABC) { control_mode = MODE_ABC; } else if (control_mode == MODE_SYNC_TO_IDLE && transition_interpolator->isEmpty() ) { - std::cerr << "[" << m_profile.instance_name << "] Finished cleanup" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] Finished cleanup" << std::endl; control_mode = MODE_IDLE; } + m_robot->calcForwardKinematics(); } + + // Write Outport if ( m_qRef.data.length() != 0 ) { // initialized if (is_legged_robot) { - for ( int i = 0; i < m_robot->numJoints(); i++ ){ - m_qRef.data[i] = m_robot->joint(i)->q; + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + m_qAbc.data[i] = m_robot->joint(i)->q; } } - m_qOut.write(); + m_qAbcOut.write(); } if (is_legged_robot) { // basePos @@ -499,7 +970,7 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) m_basePos.data.z = ref_basePos(2); m_basePos.tm = m_qRef.tm; // baseRpy - hrp::Vector3 baseRpy = hrp::rpyFromRot(ref_baseRot); + baseRpy = hrp::rpyFromRot(ref_baseRot); m_baseRpy.data.r = baseRpy(0); m_baseRpy.data.p = baseRpy(1); m_baseRpy.data.y = baseRpy(2); @@ -534,73 +1005,539 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) m_sbpCogOffset.data.y = sbp_cog_offset(1); m_sbpCogOffset.data.z = sbp_cog_offset(2); m_sbpCogOffset.tm = m_qRef.tm; - } - m_basePosOut.write(); - m_baseRpyOut.write(); - m_baseTformOut.write(); - m_basePoseOut.write(); - m_zmpOut.write(); - m_cogOut.write(); - m_sbpCogOffsetOut.write(); + // is stuck + m_isStuck.data = gg->is_stuck; + m_isStuck.tm = m_qRef.tm; + // use flywheel + m_useFlywheel.data = gg->get_use_roll_flywheel() || gg->get_use_pitch_flywheel(); + m_useFlywheel.tm = m_qRef.tm; + // estimated Fxy + m_estimatedFxy.data.x = gg->fxy(0); + m_estimatedFxy.data.y = gg->fxy(1); + m_estimatedFxy.data.z = gg->fxy(2); + m_estimatedFxy.tm = m_qRef.tm; + // diff foot origin ext moment + m_diffFootOriginExtMoment.data.x = st->diff_foot_origin_ext_moment(0); + m_diffFootOriginExtMoment.data.y = st->diff_foot_origin_ext_moment(1); + m_diffFootOriginExtMoment.data.z = st->diff_foot_origin_ext_moment(2); + m_diffFootOriginExtMoment.tm = m_qRef.tm; + // write + m_basePosOut.write(); + m_baseRpyOut.write(); + m_baseTformOut.write(); + m_basePoseOut.write(); + m_zmpOut.write(); + m_cogOut.write(); + m_sbpCogOffsetOut.write(); + m_isStuckOut.write(); + m_useFlywheelOut.write(); + m_estimatedFxyOut.write(); + m_diffFootOriginExtMomentOut.write(); + for (size_t i = 0; i < 21; i++) { + m_tmp.data[i] = gg->get_tmp(i); + } + m_tmp.data[19] = ref_cog(0) - m_tmp.data[19]; + m_tmp.data[20] = ref_cog(1) - m_tmp.data[20]; + hrp::Vector3 tmp_zmp = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_zmp; + m_tmp.data[21] = tmp_zmp(0); + m_tmp.data[22] = tmp_zmp(1); + // tmp_zmp = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cmp; + // m_tmp.data[23] = tmp_zmp(0); // cmp + // m_tmp.data[24] = tmp_zmp(1); // cmp + tmp_zmp = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cmp; + m_tmp.data[23] = gg->get_tmp(21); + m_tmp.data[24] = gg->get_tmp(22); + m_tmp.data[32] = gg->debug_steppable_height; + m_tmp.tm = m_qRef.tm; + m_tmpOut.write(); + if (gg_is_walking) { + m_currentLandingPos.data.x = gg->get_current_landing_pos(0); + m_currentLandingPos.data.y = gg->get_current_landing_pos(1); + m_currentLandingPos.data.z = gg->get_current_landing_pos(2); + } else { + m_currentLandingPos.data.x = 0.0; + m_currentLandingPos.data.y = 0.0; + m_currentLandingPos.data.z = 0.0; + } + m_currentLandingPos.tm = m_qRef.tm; + m_currentLandingPosOut.write(); + for (size_t i = 0; i < st->stikp.size(); i++) { + for (size_t j = 0; j < 3; j++) { + m_allEEComp.data[6*i+j] = st->stikp[i].d_pos_swing(j); + m_allEEComp.data[6*i+j+3] = st->stikp[i].d_rpy_swing(j); + } + } + m_allEEComp.tm = m_qRef.tm; + m_allEECompOut.write(); - // reference acceleration - hrp::Sensor* sen = m_robot->sensor("gyrometer"); - if (sen != NULL) { - hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos; - hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt; - // convert to imu sensor local acceleration - hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt; - m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2); - m_accRefOut.write(); - prev_imu_sensor_pos = imu_sensor_pos; - prev_imu_sensor_vel = imu_sensor_vel; - } + // reference acceleration + hrp::Sensor* sen = m_robot->sensor("gyrometer"); + if (sen != NULL) { + hrp::Vector3 imu_sensor_pos = sen->link->p + sen->link->R * sen->localPos; + hrp::Vector3 imu_sensor_vel = (imu_sensor_pos - prev_imu_sensor_pos)/m_dt; + // convert to imu sensor local acceleration + hrp::Vector3 acc = (sen->link->R * sen->localR).transpose() * (imu_sensor_vel - prev_imu_sensor_vel)/m_dt; + m_accRef.data.ax = acc(0); m_accRef.data.ay = acc(1); m_accRef.data.az = acc(2); + m_accRefOut.write(); + prev_imu_sensor_pos = imu_sensor_pos; + prev_imu_sensor_vel = imu_sensor_vel; + } - // control parameters - m_contactStates.tm = m_qRef.tm; - m_contactStatesOut.write(); - m_controlSwingSupportTime.tm = m_qRef.tm; - m_controlSwingSupportTimeOut.write(); - m_walkingStates.data = gg_is_walking; - m_walkingStates.tm = m_qRef.tm; - m_walkingStatesOut.write(); + // control parameters + m_contactStates.tm = m_qRef.tm; + m_contactStatesOut.write(); + m_controlSwingSupportTime.tm = m_qRef.tm; + m_controlSwingSupportTimeOut.write(); + m_toeheelRatio.tm = m_qRef.tm; + m_toeheelRatioOut.write(); + m_walkingStates.data = gg_is_walking; + m_walkingStates.tm = m_qRef.tm; + m_walkingStatesOut.write(); - for (unsigned int i=0; iwrite(); + for (unsigned int i=0; iwrite(); + } + + for (unsigned int i=0; iwrite(); + } } - for (unsigned int i=0; iwrite(); + if (gg_is_walking) { + hrp::Vector3 off = hrp::Vector3(0.0, 0.0, 0.0); + hrp::Vector3 pos, vel; + int l_r; // rleg: 0, lleg: 1 + gg->get_rel_landing_pos(pos, l_r); + m_landingTarget.tm = m_qRef.tm; + m_landingTarget.data.x = pos(0) + off(0); + m_landingTarget.data.y = pos(1) + off(1); + m_landingTarget.data.z = pos(2) + off(2); + m_landingTarget.data.l_r = l_r; + m_landingTargetOut.write(); + gg->get_end_cog_state(pos, vel, l_r); + m_endCogState.tm = m_qRef.tm; + m_endCogState.data.x = pos(0); + m_endCogState.data.y = pos(1); + m_endCogState.data.z = pos(2); + m_endCogState.data.vx = vel(0); + m_endCogState.data.vy = vel(1); + m_endCogState.data.vz = vel(2); + m_endCogState.data.l_r = l_r; + m_endCogStateOut.write(); } + setABCData2ST(); + st->execStabilizer(); + + if (!st->reset_emergency_flag && st->is_emergency) { + m_emergencySignal.data = 1; + m_emergencySignalOut.write(); + } + if ( m_qRef.data.length() != 0 ) { // initialized + if (is_legged_robot) { + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + m_qRef.data[i] = m_robot->joint(i)->q; + m_tau.data[i] = m_robot->joint(i)->u; + } + } + m_qOut.write(); + m_tau.tm = m_qRef.tm; + m_tauOut.write(); + } + // for debug output + m_originRefZmp.data.x = st->ref_zmp(0); m_originRefZmp.data.y = st->ref_zmp(1); m_originRefZmp.data.z = st->ref_zmp(2); + m_originRefCog.data.x = st->ref_cog(0); m_originRefCog.data.y = st->ref_cog(1); m_originRefCog.data.z = st->ref_cog(2); + m_originRefCogVel.data.x = st->ref_cogvel(0); m_originRefCogVel.data.y = st->ref_cogvel(1); m_originRefCogVel.data.z = st->ref_cogvel(2); + m_originNewZmp.data.x = st->new_refzmp(0); m_originNewZmp.data.y = st->new_refzmp(1); m_originNewZmp.data.z = st->new_refzmp(2); + m_originActZmp.data.x = st->act_zmp(0); m_originActZmp.data.y = st->act_zmp(1); m_originActZmp.data.z = st->act_zmp(2); + m_originActCog.data.x = st->act_cog(0); m_originActCog.data.y = st->act_cog(1); m_originActCog.data.z = st->act_cog(2); + m_originActCogVel.data.x = st->act_cogvel(0); m_originActCogVel.data.y = st->act_cogvel(1); m_originActCogVel.data.z = st->act_cogvel(2); + m_zmpAct.data.x = st->rel_act_zmp(0); m_zmpAct.data.y = st->rel_act_zmp(1); m_zmpAct.data.z = st->rel_act_zmp(2); + m_refCP.data.x = st->rel_ref_cp(0); m_refCP.data.y = st->rel_ref_cp(1); m_refCP.data.z = st->rel_ref_cp(2); + m_actCP.data.x = st->rel_act_cp(0); m_actCP.data.y = st->rel_act_cp(1); m_actCP.data.z = st->rel_act_cp(2); + for (size_t i = 0; i < m_actContactStates.data.length(); i++) m_actContactStates.data[i] = st->act_contact_states[i]; + for (size_t i = 0; i < m_COPInfo.data.length(); i++) m_COPInfo.data[i] = st->copInfo[i]; + if (debug_read_steppable_region && gg_is_walking) { + gg->get_current_steppable_region(m_currentSteppableRegion); + m_currentSteppableRegion.tm = m_qRef.tm; + m_currentSteppableRegionOut.write(); + } + m_originRefZmp.tm = m_qRef.tm; + m_originRefZmpOut.write(); + m_originRefCog.tm = m_qRef.tm; + m_originRefCogOut.write(); + m_originRefCogVel.tm = m_qRef.tm; + m_originRefCogVelOut.write(); + m_originNewZmp.tm = m_qRef.tm; + m_originNewZmpOut.write(); + m_originActZmp.tm = m_qRef.tm; + m_originActZmpOut.write(); + m_originActCog.tm = m_qRef.tm; + m_originActCogOut.write(); + m_originActCogVel.tm = m_qRef.tm; + m_originActCogVelOut.write(); + m_zmpAct.tm = m_qRef.tm; + m_zmpActOut.write(); + m_refCP.tm = m_qRef.tm; + m_refCPOut.write(); + m_actCP.tm = m_qRef.tm; + m_actCPOut.write(); + m_actContactStates.tm = m_qRef.tm; + m_actContactStatesOut.write(); + m_COPInfo.tm = m_qRef.tm; + m_COPInfoOut.write(); return RTC::RTC_OK; } -void AutoBalancer::getCurrentParameters() +void AutoBalancer::setActData2ST() { - current_root_p = m_robot->rootLink()->p; - current_root_R = m_robot->rootLink()->R; - for ( int i = 0; i < m_robot->numJoints(); i++ ){ - qorg[i] = m_robot->joint(i)->q; + for ( int i = 0; i < m_robot->numJoints(); i++ ) { + st->qCurrent[i] = m_qCurrent.data[i]; + } + for (size_t j = 0; j < st->wrenches.size(); j++) { + for (size_t i = 0; i < 6; i++) { + st->wrenches[j][i] = m_wrenches[j].data[i]; + } } + st->rpy(0) = m_rpy.data.r; + st->rpy(1) = m_rpy.data.p; + st->rpy(2) = m_rpy.data.y; +} + +void AutoBalancer::setABCData2ST() +{ + for ( int i = 0; i < m_robot->numJoints(); i++ ) { + st->qRef[i] = m_robot->joint(i)->q; + st->qRefSeq[i] = m_qRefSeq.data[i]; + } + for (size_t i = 0; i < m_contactStates.data.length(); i++) { + st->ref_contact_states[i] = m_contactStates.data[i]; + st->toeheel_ratio[i] = m_toeheelRatio.data[i]; + st->controlSwingSupportTime[i] = m_controlSwingSupportTime.data[i]; + } + for (size_t j = 0; j < st->wrenches.size(); j++) { + for (size_t i = 0; i < 6; i++) { + st->ref_wrenches[j][i] = m_force[j].data[i]; + } + } + for (size_t i = 0; i < st->limbCOPOffset.size(); i++) { + st->limbCOPOffset[i](0) = m_limbCOPOffset[i].data.x; + st->limbCOPOffset[i](1) = m_limbCOPOffset[i].data.y; + st->limbCOPOffset[i](2) = m_limbCOPOffset[i].data.z; + st->stikp[i].localCOPPos = st->stikp[i].localp + st->stikp[i].localR * hrp::Vector3(st->limbCOPOffset[i](0), 0, st->limbCOPOffset[i](2)); + } + st->zmpRef = rel_ref_zmp; + st->basePos = ref_basePos; + st->baseRpy = baseRpy; + st->is_walking = gg_is_walking || gg->is_wheeling || gg->is_jumping; + st->is_single_walking = gg_is_walking && gg->get_is_single_walking(); + st->sbp_cog_offset = sbp_cog_offset; } void AutoBalancer::getTargetParameters() { // joint angles - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qRef.data[i]; - qrefv[i] = m_robot->joint(i)->q; } + fik->setReferenceJointAngles(); // basepos, rot, zmp m_robot->rootLink()->p = input_basePos; m_robot->rootLink()->R = input_baseRot; m_robot->calcForwardKinematics(); - // + gg->proc_zmp_weight_map_interpolation(); + gg->set_total_mass(m_robot->totalMass()); + // Set force + for (unsigned int i=0; i< m_force.size(); i++) { + for (unsigned int j=0; j<6; j++) { + m_force[i].data[j] = m_ref_force[i].data[j]; + } + } if (control_mode != MODE_IDLE) { + interpolateLegNamesAndZMPOffsets(); + // Calculate tmp_fix_coords and something coordinates tmp_fix_coords; + // calc convex_hull // assume 2 legs + { + std::vector tmp_contact_states(2); + std::vector tmp_ee_pos(2); + std::vector tmp_ee_rot(2); + for (size_t i = 0; i < 2; i++) { + ABCIKparam& tmpikp = ikp[leg_names[i]]; + tmp_ee_pos[i] = tmpikp.target_p0; + tmp_ee_rot[i] = tmpikp.target_r0; + tmp_contact_states[i] = (gg->is_inverse_double_phase ? true : m_contactStates.data[i]); + } + gg->get_vertices(support_polygon_vertices); + gg->calc_convex_hull(support_polygon_vertices, tmp_contact_states, tmp_ee_pos, tmp_ee_rot); + } + if ( gg_is_walking || gg->is_wheeling || gg->is_jumping ) { + gg->set_default_zmp_offsets(default_zmp_offsets); + hrp::Vector3 act_cog = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cog; + act_cog.head(2) += sbp_cog_offset.head(2); + hrp::Vector3 act_cogvel = st->ref_foot_origin_rot * st->act_cogvel; + if (gg_is_walking) { + gg_solved = gg->proc_one_tick(act_cog, act_cogvel, target_cog); + if (!gg_solved) { + if (!gg->is_wheeling) stopWalking(); + else { + gg->clear_footstep_nodes_list(); + gg_is_walking = false; + } + } + st->falling_direction = gg->get_falling_direction(); + gg->get_swing_support_mid_coords(tmp_fix_coords); + } else if (gg->is_jumping) { + if (!gg->proc_one_tick_jump(act_cog, act_cogvel, target_cog)) stopJumping(); + gg->get_jump_mid_coords(tmp_fix_coords); + } + if (!gg_solved && gg->is_wheeling) { + if (!gg->proc_one_tick_wheel(act_cog, act_cogvel, target_cog)) stopWheeling(); + gg->get_wheel_mid_coords(tmp_fix_coords); + } + } else { + tmp_fix_coords = fix_leg_coords; + initial_fix_coords = tmp_fix_coords; + } + if (!adjust_footstep_interpolator->isEmpty()) { + calcFixCoordsForAdjustFootstep(tmp_fix_coords); + fix_leg_coords = tmp_fix_coords; + } + // TODO : see explanation in this function + fixLegToCoords2(tmp_fix_coords); + fix_leg_coords2 = tmp_fix_coords; + + // Get output parameters and target EE coords + target_root_p = m_robot->rootLink()->p; + target_root_R = m_robot->rootLink()->R; + if ( gg_is_walking ) { + getOutputParametersForWalking(); + } else { + getOutputParametersForABC(); + if (gg->is_jumping) getOutputParametersForJumping(); + if (gg->is_wheeling) getOutputParametersForWheeling(); + } + // Just for ik initial value + if (control_mode == MODE_SYNC_TO_ABC) { + fik->current_root_p = target_root_p; + fik->current_root_R = target_root_R; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) { + it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos; + it->second.target_r0 = it->second.target_link->R * it->second.localR; + } + } + } + + // Calculate other parameters + updateTargetCoordsForHandFixMode (tmp_fix_coords); + rotateRefForcesForFixCoords (tmp_fix_coords); + // TODO : see explanation in this function + // calculateOutputRefForces (); + // TODO : see explanation in this function + updateWalkingVelocityFromHandError(tmp_fix_coords); + calcReferenceJointAnglesForIK(); + + calcTouchoffRemainTime(); + + // Calculate ZMP, COG, and sbp targets + hrp::Vector3 tmp_ref_cog(m_robot->calcCM()); + hrp::Vector3 tmp_foot_mid_pos = calcFootMidPosUsingZMPWeightMap (); + target_cog = tmp_foot_mid_pos; + target_cog(2) = (tmp_ref_cog - tmp_fix_coords.pos + initial_fix_coords.pos)(2); + if (gg_is_walking || gg->is_jumping || gg->is_wheeling) { + prev_orig_cog = orig_cog; + ref_cog = gg->get_cog(); + orig_cog = ref_cog; + } else { + ref_cog = target_cog; + } + limit_cog(ref_cog); + if (gg_is_walking || gg->is_jumping || gg->is_wheeling) { + ref_zmp = gg->get_refzmp(); + } else { + ref_zmp(0) = ref_cog(0); + ref_zmp(1) = ref_cog(1); + ref_zmp(2) = tmp_foot_mid_pos(2); + } + } else { // MODE_IDLE + // Update fix_leg_coords based on input basePos and rot if MODE_IDLE + std::vector tmp_end_coords_list; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) { + tmp_end_coords_list.push_back(coordinates(it->second.target_link->p+it->second.target_link->R*it->second.localPos, it->second.target_link->R*it->second.localR)); + } + } + multi_mid_coords(fix_leg_coords, tmp_end_coords_list); + getOutputParametersForIDLE(); + is_after_walking = false; + } +}; + +void AutoBalancer::getOutputParametersForWalking () +{ + int contact_cnt = 0; + hrp::Vector3 total_force = st->ref_foot_origin_rot.transpose() * (m_robot->totalMass() * gg->get_cog_acc()); + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + // Check whether "it->first" ee_name is included in leg_names. leg_names is equivalent to "swing" + "support" in gg. + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + // Set EE coords + gg->get_swing_support_ee_coords_from_ee_name(it->second.target_p0, it->second.target_r0, it->first); + // Set contactStates + m_contactStates.data[idx] = gg->get_current_support_state_from_ee_name(it->first); + if (m_contactStates.data[idx]) contact_cnt++; + // Set controlSwingSupportTime + m_controlSwingSupportTime.data[idx] = gg->get_current_swing_time_from_ee_name(it->first); + // Set limbCOPOffset + hrp::Vector3 tmpzmpoff(m_limbCOPOffset[idx].data.x, m_limbCOPOffset[idx].data.y, m_limbCOPOffset[idx].data.z); + gg->get_swing_support_foot_zmp_offsets_from_ee_name(tmpzmpoff, it->first); + m_limbCOPOffset[idx].data.x = tmpzmpoff(0); + m_limbCOPOffset[idx].data.y = tmpzmpoff(1); + m_limbCOPOffset[idx].data.z = tmpzmpoff(2); + // Set toe heel ratio which can be used force moment distribution + double tmp = m_toeheelRatio.data[idx]; + gg->get_current_toe_heel_ratio_from_ee_name(tmp, it->first); + m_toeheelRatio.data[idx] = tmp; + } else { // Not included in leg_names + // Set EE coords + it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos; + it->second.target_r0 = it->second.target_link->R * it->second.localR; + // contactStates is OFF other than leg_names + m_contactStates.data[idx] = false; + // controlSwingSupportTime is not used while double support period, 1.0 is neglected + m_controlSwingSupportTime.data[idx] = 1.0; + // Set limbCOPOffset + m_limbCOPOffset[idx].data.x = default_zmp_offsets[idx](0); + m_limbCOPOffset[idx].data.y = default_zmp_offsets[idx](1); + m_limbCOPOffset[idx].data.z = default_zmp_offsets[idx](2); + // Set toe heel ratio which can be used force moment distribution + m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio; + total_force -= hrp::Vector3(m_force[idx].data[0], m_force[idx].data[1], m_force[idx].data[2]); + } + } + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + for (size_t i = 0; i < 3; i++) { + if (m_contactStates.data[idx]) m_force[idx].data[i] = total_force(i) / static_cast(contact_cnt); + else m_force[idx].data[i] = 0.0; + } + } + } +}; + +void AutoBalancer::getOutputParametersForABC () +{ + int contact_cnt = 0; + hrp::Vector3 total_force = st->ref_foot_origin_rot.transpose() * (m_robot->totalMass() * hrp::Vector3(0, 0, gg->get_gravitational_acceleration())); + // double support by default + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + // Set EE coords. + // If not included in leg_names, calculate EE coords because of not being used in GaitGenerator. + // Otherwise, keep previous EE coords derived from GaitGenerator or initial value. + if ( std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) { + it->second.target_p0 = it->second.target_link->p + it->second.target_link->R * it->second.localPos; + it->second.target_r0 = it->second.target_link->R * it->second.localR; + } + // Set contactStates + std::vector::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first)); + if (dst != leg_names.end()) { + m_contactStates.data[idx] = true; + contact_cnt++; + } else { + m_contactStates.data[idx] = false; + total_force -= hrp::Vector3(m_force[idx].data[0], m_force[idx].data[1], m_force[idx].data[2]); + } + // controlSwingSupportTime is not used while double support period, 1.0 is neglected + m_controlSwingSupportTime.data[idx] = 1.0; + // Set limbCOPOffset + m_limbCOPOffset[idx].data.x = default_zmp_offsets[idx](0); + m_limbCOPOffset[idx].data.y = default_zmp_offsets[idx](1); + m_limbCOPOffset[idx].data.z = default_zmp_offsets[idx](2); + // Set toe heel ratio is not used while double support + m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio; + } + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + for (size_t i = 0; i < 3; i++) { + if (m_contactStates.data[idx]) m_force[idx].data[i] = total_force(i) / static_cast(contact_cnt); + else m_force[idx].data[i] = 0.0; + } + } + } +}; + +void AutoBalancer::getOutputParametersForJumping () +{ + int contact_cnt = 0; + hrp::Vector3 total_force = st->ref_foot_origin_rot.transpose() * (m_robot->totalMass() * gg->get_cog_acc()); + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + // if (gg->is_jumping_phase()) m_contactStates.data[idx] = false; // TODO: fix bug on st + if (m_contactStates.data[idx]) contact_cnt++; + // Check whether "it->first" ee_name is included in leg_names. leg_names is equivalent to "swing" + "support" in gg. + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + gg->get_jump_ee_coords_from_ee_name(it->second.target_p0, it->second.target_r0, it->first); + } else { + total_force -= hrp::Vector3(m_force[idx].data[0], m_force[idx].data[1], m_force[idx].data[2]); + } + } + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + for (size_t i = 0; i < 3; i++) { + if (m_contactStates.data[idx]) m_force[idx].data[i] = total_force(i) / static_cast(contact_cnt); + else m_force[idx].data[i] = 0.0; + } + } + } +} + +void AutoBalancer::getOutputParametersForWheeling () +{ + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + // Check whether "it->first" ee_name is included in leg_names. leg_names is equivalent to "swing" + "support" in gg. + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + gg->get_wheel_ee_coords_from_ee_name(it->second.target_p0, it->second.target_r0, it->first); + } + } +} + +void AutoBalancer::getOutputParametersForIDLE () +{ + // Set contactStates and controlSwingSupportTime + if (m_optionalData.data.length() >= contact_states_index_map.size()*2) { + // current optionalData is contactstates x limb and controlSwingSupportTime x limb + // If contactStates in optionalData is 1.0, m_contactStates is true. Otherwise, false. + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + m_contactStates.data[contact_states_index_map[it->first]] = isOptionalDataContact(it->first); + m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = m_optionalData.data[contact_states_index_map[it->first]+contact_states_index_map.size()]; + } + // If two feet have no contact, force set double support contact + if ( !m_contactStates.data[contact_states_index_map["rleg"]] && !m_contactStates.data[contact_states_index_map["lleg"]] ) { + m_contactStates.data[contact_states_index_map["rleg"]] = true; + m_contactStates.data[contact_states_index_map["lleg"]] = true; + } + } + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + size_t idx = contact_states_index_map[it->first]; + // Set limbCOPOffset + m_limbCOPOffset[idx].data.x = 0; + m_limbCOPOffset[idx].data.y = 0; + m_limbCOPOffset[idx].data.z = 0; + // Set toe heel ratio is not used while double support + m_toeheelRatio.data[idx] = rats::no_using_toe_heel_ratio; + } +}; + +void AutoBalancer::interpolateLegNamesAndZMPOffsets() +{ if (!zmp_offset_interpolator->isEmpty()) { double *default_zmp_offsets_output = new double[ikp.size()*3]; zmp_offset_interpolator->get(default_zmp_offsets_output, true); @@ -610,10 +1547,8 @@ void AutoBalancer::getTargetParameters() delete[] default_zmp_offsets_output; if (DEBUGP) { std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl; - std::map leg_type_map = gg->get_leg_type_map(); - for (size_t i = 0; i < leg_names.size(); i++) { - std::map::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == leg_names[i])); - std::cerr << "[" << m_profile.instance_name << "] " << leg_names[i] << " = " << default_zmp_offsets[dst->first].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + std::cerr << "[" << m_profile.instance_name << "] " << it->first << " = " << default_zmp_offsets[contact_states_index_map[it->first]].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; } } } @@ -622,178 +1557,53 @@ void AutoBalancer::getTargetParameters() }else { leg_names_interpolator_ratio = 1.0; } - if ( gg_is_walking ) { - gg->set_default_zmp_offsets(default_zmp_offsets); - gg_solved = gg->proc_one_tick(); - { - std::map leg_type_map = gg->get_leg_type_map(); - coordinates tmpc; - // for support leg - for (std::vector::const_iterator it = gg->get_support_leg_steps().begin(); it != gg->get_support_leg_steps().end(); it++) { - coordinates sp_coords = it->worldcoords; - coordinates(ikp[leg_type_map[it->l_r]].localPos, - ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc); - sp_coords.transform(tmpc); - ikp[leg_type_map[it->l_r]].target_p0 = sp_coords.pos; - ikp[leg_type_map[it->l_r]].target_r0 = sp_coords.rot; - } - // for swing leg - for (std::vector::const_iterator it = gg->get_swing_leg_steps().begin(); it != gg->get_swing_leg_steps().end(); it++) { - coordinates sw_coords = it->worldcoords; - coordinates(ikp[leg_type_map[it->l_r]].localPos, - ikp[leg_type_map[it->l_r]].localR).inverse_transformation(tmpc); - sw_coords.transform(tmpc); - ikp[leg_type_map[it->l_r]].target_p0 = sw_coords.pos; - ikp[leg_type_map[it->l_r]].target_r0 = sw_coords.rot; - } - } - gg->get_swing_support_mid_coords(tmp_fix_coords); - // set contactStates - { - std::vector tmp_current_support_states_names; - { - std::vector tmp_current_support_states = gg->get_current_support_states(); - std::map leg_type_map = gg->get_leg_type_map(); - for (std::vector::const_iterator it = tmp_current_support_states.begin(); it != tmp_current_support_states.end(); it++) - tmp_current_support_states_names.push_back(leg_type_map[*it]); - } - // Set Contact States for ee not included in leg_names to false - for ( std::map::iterator it = contact_states_index_map.begin(); it != contact_states_index_map.end(); it++ ) { - m_contactStates.data[it->second] = false; - } - for (std::vector::const_iterator it = leg_names.begin(); it != leg_names.end(); it++) { - std::vector::const_iterator dst = std::find_if(tmp_current_support_states_names.begin(), tmp_current_support_states_names.end(), boost::lambda::_1 == *it); - if (dst != tmp_current_support_states_names.end()) { - m_contactStates.data[contact_states_index_map[*it]] = true; - } else { - m_contactStates.data[contact_states_index_map[*it]] = false; - } - } - } - // set controlSwingSupportTime - { - std::map leg_type_map = gg->get_leg_type_map(); - for (std::map::const_iterator it = ikp.begin(); it != ikp.end(); it++) { - std::map::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == it->first)); - m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = gg->get_current_swing_time(dst->first); - } - } - // set ref_forces - { - std::vector ee_pos; - for (size_t i = 0 ; i < leg_names.size(); i++) { - ABCIKparam& tmpikp = ikp[leg_names[i]]; - ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]); - } - double alpha = (ref_zmp - ee_pos[1]).norm() / (ee_pos[0] - ee_pos[1]).norm(); - if (alpha>1.0) alpha = 1.0; - if (alpha<0.0) alpha = 0.0; - if (DEBUGP) { - std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl; - } - double mg = m_robot->totalMass() * gg->get_gravitational_acceleration(); - m_force[0].data[0] = alpha * mg; - m_force[1].data[0] = (1-alpha) * mg; - } - // set limbCOPOffset - { - std::vector swg_leg_nms = gg->get_swing_leg_names(); - for (size_t i = 0; i < swg_leg_nms.size(); i++) { - m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.x = gg->get_swing_foot_zmp_offsets().at(i)(0); - m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.y = gg->get_swing_foot_zmp_offsets().at(i)(1); - m_limbCOPOffset[contact_states_index_map[swg_leg_nms.at(i)]].data.z = gg->get_swing_foot_zmp_offsets().at(i)(2); - } - } - { - std::vector sup_leg_nms = gg->get_support_leg_names(); - for (size_t i = 0; i < sup_leg_nms.size(); i++) { - m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.x = gg->get_support_foot_zmp_offsets().at(i)(0); - m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.y = gg->get_support_foot_zmp_offsets().at(i)(1); - m_limbCOPOffset[contact_states_index_map[sup_leg_nms.at(i)]].data.z = gg->get_support_foot_zmp_offsets().at(i)(2); - } - } - } else { - tmp_fix_coords = fix_leg_coords; - // double support by default - { - std::map leg_type_map = gg->get_leg_type_map(); - for (std::map::const_iterator it = ikp.begin(); it != ikp.end(); it++) { - std::vector::const_iterator dst = std::find_if(leg_names.begin(), leg_names.end(), (boost::lambda::_1 == it->first)); - if (dst != leg_names.end()) { - m_contactStates.data[contact_states_index_map[it->first]] = true; - } else { - m_contactStates.data[contact_states_index_map[it->first]] = false; - } - // controlSwingSupportTime is not used while double support period, 1.0 is neglected - m_controlSwingSupportTime.data[contact_states_index_map[it->first]] = 1.0; - std::map::const_iterator dst2 = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == it->first)); - m_limbCOPOffset[contact_states_index_map[it->first]].data.x = default_zmp_offsets.at(dst2->first)(0); - m_limbCOPOffset[contact_states_index_map[it->first]].data.y = default_zmp_offsets.at(dst2->first)(1); - m_limbCOPOffset[contact_states_index_map[it->first]].data.z = default_zmp_offsets.at(dst2->first)(2); - } - } - } - if (!adjust_footstep_interpolator->isEmpty()) { - double tmp = 0.0; - adjust_footstep_interpolator->get(&tmp, true); - //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl; - ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0; - ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0; - rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0); - rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0); - coordinates tmprc, tmplc; - tmprc.pos = ikp["rleg"].target_p0 + ikp["rleg"].target_r0 * ikp["rleg"].localPos; - tmprc.rot = ikp["rleg"].target_r0 * ikp["rleg"].localR; - tmplc.pos = ikp["lleg"].target_p0 + ikp["lleg"].target_r0 * ikp["lleg"].localPos; - tmplc.rot = ikp["lleg"].target_r0 * ikp["lleg"].localR; - rats::mid_coords(fix_leg_coords, 0.5, tmprc, tmplc); - tmp_fix_coords = fix_leg_coords; - } - // Tempolarily modify tmp_fix_coords - // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 - { - hrp::Vector3 ex = hrp::Vector3::UnitX(); - hrp::Vector3 ez = hrp::Vector3::UnitZ(); - hrp::Vector3 xv1 (tmp_fix_coords.rot * ex); - xv1(2) = 0.0; - xv1.normalize(); - hrp::Vector3 yv1(ez.cross(xv1)); - tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2); - tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2); - tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2); - } - // Fix pos - fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot); +}; + +void AutoBalancer::calcFixCoordsForAdjustFootstep (coordinates& tmp_fix_coords) +{ + double tmp = 0.0; + adjust_footstep_interpolator->get(&tmp, true); + //std::cerr << "[" << m_profile.instance_name << "] adjust ratio " << tmp << std::endl; + ikp["rleg"].target_p0 = (1-tmp) * ikp["rleg"].adjust_interpolation_org_p0 + tmp*ikp["rleg"].adjust_interpolation_target_p0; + ikp["lleg"].target_p0 = (1-tmp) * ikp["lleg"].adjust_interpolation_org_p0 + tmp*ikp["lleg"].adjust_interpolation_target_p0; + rats::mid_rot(ikp["rleg"].target_r0, tmp, ikp["rleg"].adjust_interpolation_org_r0, ikp["rleg"].adjust_interpolation_target_r0); + rats::mid_rot(ikp["lleg"].target_r0, tmp, ikp["lleg"].adjust_interpolation_org_r0, ikp["lleg"].adjust_interpolation_target_r0); + coordinates tmprc, tmplc; + tmprc.pos = ikp["rleg"].target_p0; + tmprc.rot = ikp["rleg"].target_r0; + tmplc.pos = ikp["lleg"].target_p0; + tmplc.rot = ikp["lleg"].target_r0; + rats::mid_coords(tmp_fix_coords, 0.5, tmprc, tmplc); + //fix_leg_coords = tmp_fix_coords; +}; - /* update ref_forces ;; sp's absolute -> rmc's absolute */ +void AutoBalancer::rotateRefForcesForFixCoords (coordinates& tmp_fix_coords) +{ + /* update ref_forces ;; StateHolder's absolute -> AutoBalancer's absolute */ for (size_t i = 0; i < m_ref_forceIn.size(); i++) { - hrp::Matrix33 eeR; - hrp::Link* parentlink; - hrp::ForceSensor* sensor = m_robot->sensor(sensor_names[i]); - if (sensor) parentlink = sensor->link; - else parentlink = m_vfs[sensor_names[i]].link; - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR; - } + // hrp::Matrix33 eeR; + // hrp::Link* parentlink; + // hrp::ForceSensor* sensor = m_robot->sensor(sensor_names[i]); + // if (sensor) parentlink = sensor->link; + // else parentlink = m_vfs[sensor_names[i]].link; + // for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + // if (it->second.target_link->name == parentlink->name) eeR = parentlink->R * it->second.localR; + // } // End effector frame //ref_forces[i] = eeR * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]); // world frame ref_forces[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[0], m_ref_force[i].data[1], m_ref_force[i].data[2]); + ref_moments[i] = tmp_fix_coords.rot * hrp::Vector3(m_ref_force[i].data[3], m_ref_force[i].data[4], m_ref_force[i].data[5]); } sbp_offset = tmp_fix_coords.rot * hrp::Vector3(sbp_offset); +}; - target_root_p = m_robot->rootLink()->p; - target_root_R = m_robot->rootLink()->R; - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if ( control_mode == MODE_IDLE || std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() ) { - it->second.target_p0 = it->second.target_link->p; - it->second.target_r0 = it->second.target_link->R; - } - } +void AutoBalancer::updateTargetCoordsForHandFixMode (coordinates& tmp_fix_coords) +{ // Move hand for hand fix mode // If arms' ABCIKparam.is_active is true, move hands according to cog velocity. // If is_hand_fix_mode is false, no hand fix mode and move hands according to cog velocity. - // If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot. + // If is_hand_fix_mode is true, hand fix mode and do not move hands in Y axis in tmp_fix_coords.rot. if (gg_is_walking) { // hand control while walking = solve hand ik with is_hand_fix_mode and solve hand ik without is_hand_fix_mode bool is_hand_control_while_walking = false; @@ -818,68 +1628,105 @@ void AutoBalancer::getTargetParameters() if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() && it->first.find("arm") != std::string::npos ) { it->second.target_p0 = it->second.target_p0 + dif_p; + it->second.handfix_target_p0 = it->second.target_p0; } } + prev_orig_dif_p = orig_dif_p; + orig_dif_p = dif_p; + } + } else if (is_after_walking) { + std::vector start_pos(3), start_vel(3), goal_pos(3); + double tmp_time = 5.0; + for (size_t i = 0; i < 3; i++) { + start_pos[i] = orig_dif_p(i); + start_vel[i] = (orig_dif_p(i) - prev_orig_dif_p(i)) / m_dt; + goal_pos[i] = 0.0; + } + hand_fix_interpolator->set(start_pos.data(), start_vel.data()); + hand_fix_interpolator->setGoal(goal_pos.data(), tmp_time, true); + } + if (!hand_fix_interpolator->isEmpty()) { + std::vector tmp_v(3); + hand_fix_interpolator->get(tmp_v.data(), true); + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if ( it->second.is_active && std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end() + && it->first.find("arm") != std::string::npos ) { + for (size_t i = 0; i < 3; i++) { + it->second.target_p0(i) = it->second.target_p0(i) + tmp_v[i]; + } } + } } +}; - hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero()); - { - std::map leg_type_map = gg->get_leg_type_map(); - std::map zmp_weight_map = gg->get_zmp_weight_map(); - double sum_of_weight = 0.0; - for (size_t i = 0; i < leg_names.size(); i++) { +void AutoBalancer::calculateOutputRefForces () +{ + // TODO : need to be updated for multicontact and other walking + if (leg_names.size() == 2) { + std::vector ee_pos; + for (size_t i = 0 ; i < leg_names.size(); i++) { ABCIKparam& tmpikp = ikp[leg_names[i]]; - // get target_end_coords - tmpikp.target_end_coords.pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos; - tmpikp.target_end_coords.rot = tmpikp.target_r0 * tmpikp.localR; - // for foot_mid_pos - std::map::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == leg_names[i])); - tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]) * zmp_weight_map[dst->first]; - sum_of_weight += zmp_weight_map[dst->first]; + ee_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * default_zmp_offsets[i]); } - tmp_foot_mid_pos *= (1.0 / sum_of_weight); + double alpha = (ref_zmp - ee_pos[1]).norm() / ((ee_pos[0] - ref_zmp).norm() + (ee_pos[1] - ref_zmp).norm()); + if (alpha>1.0) alpha = 1.0; + if (alpha<0.0) alpha = 0.0; + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] alpha:" << alpha << std::endl; + } + double mg = m_robot->totalMass() * gg->get_gravitational_acceleration(); + m_force[0].data[2] = alpha * mg; + m_force[1].data[2] = (1-alpha) * mg; } - // - { - if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) { - hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords)); - gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2)); - }// else { - // if ( gg_is_walking && gg->get_lcg_count() == static_cast(gg->get_default_step_time()/(2*m_dt))-1) { - // gg->set_offset_velocity_param(0,0,0); - // } - // } + if ( use_force == MODE_REF_FORCE_WITH_FOOT || use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) { // TODO : use other use_force mode. This should be depends on Stabilizer distribution mode. + distributeReferenceZMPToWrenches (ref_zmp); } + prev_ref_zmp = ref_zmp; +}; - // - - hrp::Vector3 tmp_ref_cog(m_robot->calcCM()); - if (gg_is_walking) { - ref_cog = gg->get_cog(); - } else { - ref_cog = tmp_foot_mid_pos; - } - ref_cog(2) = tmp_ref_cog(2); - if (gg_is_walking) { - ref_zmp = gg->get_refzmp(); - } else { - ref_zmp(0) = ref_cog(0); - ref_zmp(1) = ref_cog(1); - ref_zmp(2) = tmp_foot_mid_pos(2); +hrp::Vector3 AutoBalancer::calcFootMidPosUsingZMPWeightMap () +{ + hrp::Vector3 tmp_foot_mid_pos(hrp::Vector3::Zero()); + std::map leg_type_map = gg->get_leg_type_map(); + std::map zmp_weight_map = gg->get_zmp_weight_map(); + double sum_of_weight = 0.0; + for (size_t i = 0; i < leg_names.size(); i++) { + ABCIKparam& tmpikp = ikp[leg_names[i]]; + // for foot_mid_pos + std::map::const_iterator dst = std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == leg_names[i])); + tmp_foot_mid_pos += (tmpikp.target_p0 + tmpikp.target_r0 * default_zmp_offsets[i]) * zmp_weight_map[dst->first]; + sum_of_weight += zmp_weight_map[dst->first]; } - } - // Just for ik initial value - if (control_mode == MODE_SYNC_TO_ABC) { - current_root_p = target_root_p; - current_root_R = target_root_R; + tmp_foot_mid_pos *= (1.0 / sum_of_weight); + return tmp_foot_mid_pos; +}; + +void AutoBalancer::updateWalkingVelocityFromHandError (coordinates& tmp_fix_coords) +{ + // TODO : check frame and robot state for this calculation + if ( gg_is_walking && gg->get_lcg_count() == gg->get_overwrite_check_timing()+2 ) { + hrp::Vector3 vel_htc(calc_vel_from_hand_error(tmp_fix_coords)); + gg->set_offset_velocity_param(vel_htc(0), vel_htc(1) ,vel_htc(2)); + }// else { + // if ( gg_is_walking && gg->get_lcg_count() == static_cast(gg->get_default_step_time()/(2*m_dt))-1) { + // gg->set_offset_velocity_param(0,0,0); + // } + // } +}; + +void AutoBalancer::calcReferenceJointAnglesForIK () +{ + fik->overwrite_ref_ja_index_vec.clear(); + // Fix for toe joint for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if ( std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end() ) { - it->second.target_p0 = it->second.target_link->p; - it->second.target_r0 = it->second.target_link->R; - } + if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) { + int i = it->second.target_link->jointId; + if (gg->get_swing_leg_names().front() == it->first) { + fik->qrefv[i] = fik->qrefv[i] + -1 * gg->get_foot_dif_rot_angle(); + } + fik->overwrite_ref_ja_index_vec.push_back(i); + } } - } }; hrp::Matrix33 AutoBalancer::OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2) @@ -913,73 +1760,448 @@ void AutoBalancer::fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matri m_robot->calcForwardKinematics(); } -bool AutoBalancer::solveLimbIKforLimb (ABCIKparam& param) +void AutoBalancer::fixLegToCoords2 (coordinates& tmp_fix_coords) { - param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, transition_interpolator_ratio * leg_names_interpolator_ratio); - // IK check - hrp::Vector3 vel_p, vel_r; - vel_p = param.target_p0 - param.target_link->p; - rats::difference_rotation(vel_r, param.target_link->R, param.target_r0); - if (vel_p.norm() > pos_ik_thre && transition_interpolator->isEmpty()) { - if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) { - std::cerr << "[" << m_profile.instance_name << "] Too large IK error (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl; + // Tempolarily modify tmp_fix_coords + // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 + // Snap input tmp_fix_coords to XY plan projection. + { + hrp::Vector3 ex = hrp::Vector3::UnitX(); + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 xv1 (tmp_fix_coords.rot * ex); + xv1(2) = 0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + tmp_fix_coords.rot(0,0) = xv1(0); tmp_fix_coords.rot(1,0) = xv1(1); tmp_fix_coords.rot(2,0) = xv1(2); + tmp_fix_coords.rot(0,1) = yv1(0); tmp_fix_coords.rot(1,1) = yv1(1); tmp_fix_coords.rot(2,1) = yv1(2); + tmp_fix_coords.rot(0,2) = ez(0); tmp_fix_coords.rot(1,2) = ez(1); tmp_fix_coords.rot(2,2) = ez(2); + } + fixLegToCoords(tmp_fix_coords.pos, tmp_fix_coords.rot); +} + +// assume biped walking +void AutoBalancer::calcTouchoffRemainTime() +{ + for (size_t i = 0; i < 2; i++) { + if (gg_is_walking) { + touchoff_remain_time[i] = gg->get_touchoff_remain_count(i) * m_dt; + if (st->stikp.size() > i) st->stikp[i].touchoff_remain_time = touchoff_remain_time[i]; + } else { + touchoff_remain_time[i] = 0.0; + } + } +} + +void AutoBalancer::stopFootForEarlyTouchDown () +{ + bool is_last_double = (gg->get_footstep_index() == gg->get_step_num() - 1); + for (size_t i = 0; i < 2; i++) { + if (gg_is_walking) { + if (gg->get_footstep_index() > 0 && !is_last_double && + ((st->act_contact_states[contact_states_index_map[leg_names[i]]] && !gg->is_before_step_phase()) || ((ikp[leg_names[i]].target_p0 - prev_foot_pos[i]).norm() > 1e-2)) && // latter part is for sensor trouble + !is_foot_touch[i]) { + double tmp_ratio = 1.0; + touchdown_transition_interpolator[leg_names[i]]->clear(); + touchdown_transition_interpolator[leg_names[i]]->set(&tmp_ratio); + ikp[leg_names[i]].target_p0 = touchdown_foot_pos[i]; + tmp_ratio = 0.0; + touchdown_transition_interpolator[leg_names[i]]->setGoal(&tmp_ratio, touchoff_remain_time[i], true); + is_foot_touch[i] = true; + + leg_type lr; + if (leg_names[i] == "rleg") lr = RLEG; + else lr = LLEG; + gg->set_is_early_touch(true, lr); } - param.pos_ik_error_count++; - has_ik_failed = true; - } else { - param.pos_ik_error_count = 0; + } + if (!touchdown_transition_interpolator[leg_names[i]]->isEmpty()) { + double tmp_ratio = 0.0; + touchdown_transition_interpolator[leg_names[i]]->setGoal(&tmp_ratio, touchoff_remain_time[i], true); + touchdown_transition_interpolator[leg_names[i]]->get(&tmp_ratio, true); + ikp[leg_names[i]].target_p0 = (touchdown_foot_pos[i] + (gg->is_wheeling ? gg->get_d_wheel_pos() : hrp::Vector3::Zero())) * tmp_ratio + ikp[leg_names[i]].target_p0 * (1.0 - tmp_ratio); + } else { + is_foot_touch[i] = false; + touchdown_foot_pos[i] = ikp[leg_names[i]].target_p0; + if (gg->is_wheeling) touchdown_foot_pos[i] -= gg->get_d_wheel_pos(); + } + prev_foot_pos[i] = ikp[leg_names[i]].target_p0; + } +} +// TODO: move to fik.h +void AutoBalancer::limbStretchAvoidanceControl () { + std::vector tmp_p; + std::vector tmp_name; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if (it->first.find("leg") != std::string::npos) { + tmp_p.push_back(it->second.target_p0 + it->second.target_r0 * it->second.localPos); + tmp_name.push_back(it->first); + } } - if (vel_r.norm() > rot_ik_thre && transition_interpolator->isEmpty()) { - if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) { - std::cerr << "[" << m_profile.instance_name << "] Too large IK error (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl; + double tmp_d_root_height = 0.0, prev_d_root_height = fik->d_root_height; + if (fik->use_limb_stretch_avoidance) { + for (size_t i = 0; i < tmp_p.size(); i++) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(fik->ikp[tmp_name[i]].parent_name); + hrp::Vector3 rel_target_p = tmp_p[i] - parent_link->p; // position from parent to target link (world frame) + double limb_length_limitation = fik->ikp[tmp_name[i]].max_limb_length - fik->ikp[tmp_name[i]].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - rel_target_p(0) * rel_target_p(0) - rel_target_p(1) * rel_target_p(1); + if (rel_target_p.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_root_height = std::min(tmp_d_root_height, rel_target_p(2) + std::sqrt(tmp)); } - param.rot_ik_error_count++; - has_ik_failed = true; + } + // Change root link height depending on limb length + fik->d_root_height = tmp_d_root_height == 0.0 ? (- 1/fik->limb_stretch_avoidance_time_const * fik->d_root_height * m_dt + fik->d_root_height) : tmp_d_root_height; } else { - param.rot_ik_error_count = 0; + fik->d_root_height = - 1/fik->limb_stretch_avoidance_time_const * fik->d_root_height * m_dt + fik->d_root_height; } - return true; + vlimit(fik->d_root_height, prev_d_root_height + fik->limb_stretch_avoidance_vlimit[0], prev_d_root_height + fik->limb_stretch_avoidance_vlimit[1]); + target_root_p(2) += fik->d_root_height; + ref_cog(2) += fik->d_root_height; } - -void AutoBalancer::solveLimbIK () +void AutoBalancer::solveFullbodyIK () { - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.is_active) { - for ( int j = 0; j < it->second.manip->numJoints(); j++ ){ - int i = it->second.manip->joint(j)->jointId; - m_robot->joint(i)->q = qorg[i]; + // set desired natural pose and pullback gain + for(int i=0;inumJoints();i++) { + fik->q_ref(i) = m_qRef.data[i]; + if (is_natural_walk && gg_is_walking) { + double arm_off = (ikp["rleg"].target_r0.transpose() * (ikp["lleg"].target_p0 - ikp["rleg"].target_p0))(0) * (deg2rad(arm_swing_deg)/0.15); + if (m_robot->joint(i)->name == "R_SHOULDER_P" || m_robot->joint(i)->name == "RARM_JOINT1") fik->q_ref(i) -= arm_off; + if (m_robot->joint(i)->name == "L_SHOULDER_P" || m_robot->joint(i)->name == "LARM_JOINT1") fik->q_ref(i) += arm_off; } + } + fik->revertRobotStateToCurrentAll(); + { + hrp::Vector3 tmpcog = m_robot->calcCM(); + if (gg_is_walking && use_act_states) { + ref_cog.head(2) = (tmpcog + (ref_cog - (st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cog))).head(2); } } - m_robot->rootLink()->p = current_root_p; - m_robot->rootLink()->R = current_root_R; - m_robot->calcForwardKinematics(); + // calc sbp_cog_offset hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0); static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2)); - hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog; - dif_cog *= leg_names_interpolator_ratio; - dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2); - m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog; - m_robot->rootLink()->R = target_root_R; - // Fix for toe joint - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.is_active && it->second.has_toe_joint && gg->get_use_toe_joint()) { - int i = it->second.target_link->jointId; - if (gg->get_swing_leg_names().front() == it->first) { - m_robot->joint(i)->q = qrefv[i] + -1 * gg->get_foot_dif_rot_angle(); - } else { - m_robot->joint(i)->q = qrefv[i]; + sbp_cog_offset(2) = 0.0; + + if (gg->use_act_states && is_stop_early_foot && st->use_force_sensor) stopFootForEarlyTouchDown(); + + limbStretchAvoidanceControl(); + + std::vector ik_tgt_list; + { + IKConstraint tmp; + tmp.target_link_name = "WAIST"; + tmp.localPos = hrp::Vector3::Zero(); + tmp.localR = hrp::Matrix33::Identity(); + tmp.targetPos = target_root_p;// will be ignored by selection_vec + tmp.targetRpy = hrp::rpyFromRot(target_root_R); + tmp.constraint_weight << 0,0,0,1e-5,1e-5,1e-5; // COMMON + ik_tgt_list.push_back(tmp); + }{ + IKConstraint tmp; + tmp.target_link_name = ikp["rleg"].target_link->name; + tmp.localPos = ikp["rleg"].localPos; + tmp.localR = ikp["rleg"].localR; + tmp.targetPos = ikp["rleg"].target_p0; + tmp.targetRpy = hrp::rpyFromRot(ikp["rleg"].target_r0); + tmp.constraint_weight << 1,1,1,1,1,1; + ik_tgt_list.push_back(tmp); + }{ + IKConstraint tmp; + tmp.target_link_name = ikp["lleg"].target_link->name; + tmp.localPos = ikp["lleg"].localPos; + tmp.localR = ikp["lleg"].localR; + tmp.targetPos = ikp["lleg"].target_p0; + tmp.targetRpy = hrp::rpyFromRot(ikp["lleg"].target_r0); + tmp.constraint_weight << 1,1,1,1,1,1; + ik_tgt_list.push_back(tmp); + } + if (ikp.size() >= 4) { + if (ikp["rarm"].is_active || ikp["larm"].is_active) { + double tmp_const = (ikp["rarm"].is_active ? 1e-1 : 1e-8) * transition_interpolator_ratio; + IKConstraint tmp; + tmp.target_link_name = ikp["rarm"].target_link->name; + tmp.localPos = ikp["rarm"].localPos; + tmp.localR = ikp["rarm"].localR; + tmp.targetPos = ikp["rarm"].target_p0; + tmp.targetRpy = hrp::rpyFromRot(ikp["rarm"].target_r0); + tmp.constraint_weight << tmp_const,tmp_const,tmp_const,tmp_const,tmp_const,tmp_const; + ik_tgt_list.push_back(tmp); + } + if (ikp["rarm"].is_active || ikp["larm"].is_active) { + double tmp_const = (ikp["larm"].is_active ? 1e-1 : 1e-8) * transition_interpolator_ratio; + IKConstraint tmp; + tmp.target_link_name = ikp["larm"].target_link->name; + tmp.localPos = ikp["larm"].localPos; + tmp.localR = ikp["larm"].localR; + tmp.targetPos = ikp["larm"].target_p0; + tmp.targetRpy = hrp::rpyFromRot(ikp["larm"].target_r0); + tmp.constraint_weight << tmp_const,tmp_const,tmp_const,tmp_const,tmp_const,tmp_const; + ik_tgt_list.push_back(tmp); + } + } + // collision aoidance + if (use_collision_avoidance) { + for (size_t i = 0; i < fik->arm_collision.size(); i++) { + if (fik->arm_collision[i].is_collision) { + IKConstraint tmp; + double tmp_weight = fik->arm_collision[i].calcWeight(1e-8); + tmp.target_link_name = m_robot->joint(fik->arm_collision[i].target_id)->name; + tmp.localPos = fik->arm_collision[i].target_localp; + tmp.targetPos = m_robot->joint(fik->arm_collision[i].base_id)->p + m_robot->joint(fik->arm_collision[i].base_id)->R * fik->arm_collision[i].base_localp + + fik->arm_collision[i].dir * (fik->arm_collision[i].safe_d0 + fik->arm_collision[i].safe_d_offset); + tmp.constraint_weight << tmp_weight,tmp_weight,tmp_weight,0,0,0; + ik_tgt_list.push_back(tmp); + } + } + } + { + IKConstraint tmp; + tmp.target_link_name = "COM"; + tmp.localPos = hrp::Vector3::Zero(); + tmp.localR = hrp::Matrix33::Identity(); + tmp.targetPos = ref_cog - sbp_cog_offset;// COM height will not be constraint + hrp::Vector3 tmp_tau = gg->get_flywheel_tau(); + tmp_tau = st->vlimit(tmp_tau, -40, 40); + tmp.targetRpy = hrp::Vector3::Zero(); + + hrp::Vector3 prev_momentum = fik->cur_momentum_around_COM_filtered; + m_tmp.data[25] = prev_momentum(0); + m_tmp.data[26] = prev_momentum(1); + fik->q_ref_err_thre = deg2rad(40.0); + if (fik->is_exceed_q_ref_err_thre) { + was_exceed_q_ref_err_thre = true; + } else if (!gg->get_use_roll_flywheel() && !gg->get_use_pitch_flywheel()) { + was_exceed_q_ref_err_thre = false; + } + bool use_roll_flywheel = gg->get_use_roll_flywheel() && !was_exceed_q_ref_err_thre, use_pitch_flywheel = gg->get_use_pitch_flywheel() && !was_exceed_q_ref_err_thre; + if (use_roll_flywheel) tmp.targetRpy(0) = (prev_momentum + tmp_tau * m_dt)(0);//reference angular momentum + if (use_pitch_flywheel) tmp.targetRpy(1) = (prev_momentum + tmp_tau * m_dt)(1);//reference angular momentum + double roll_weight, pitch_weight, fly_weight = 1e-3, normal_weight = 1e-7, weight_fly_interpolator_time = 0.1, weight_normal_interpolator_time = 0.4; + if (gg_is_walking && is_natural_walk) normal_weight = 0.0; + if (gg->is_jumping) fly_weight = 1; + if (ikp.size() >= 4 && (ikp["rarm"].is_active || ikp["larm"].is_active)) fly_weight = 1e-6; + // roll + if (use_roll_flywheel || gg->is_jumping) { + if (!prev_roll_state) { + if (roll_weight_interpolator->isEmpty()) roll_weight = normal_weight; + else roll_weight_interpolator->get(&roll_weight, true); + roll_weight_interpolator->set(&roll_weight); + roll_weight_interpolator->setGoal(&fly_weight, weight_fly_interpolator_time, true); } + if (roll_weight_interpolator->isEmpty()) roll_weight = fly_weight; + else roll_weight_interpolator->get(&roll_weight, true); + } else { + if (prev_roll_state) { + if (roll_weight_interpolator->isEmpty()) roll_weight = fly_weight; + else roll_weight_interpolator->get(&roll_weight, true); + roll_weight_interpolator->set(&roll_weight); + roll_weight_interpolator->setGoal(&normal_weight, weight_normal_interpolator_time, true); + double zero_momentum = 0.0; + roll_momentum_interpolator->set(&prev_momentum(0)); + roll_momentum_interpolator->setGoal(&zero_momentum, weight_normal_interpolator_time, true); + } + if (roll_weight_interpolator->isEmpty()) roll_weight = normal_weight; + else roll_weight_interpolator->get(&roll_weight, true); + if (!roll_momentum_interpolator->isEmpty()) roll_momentum_interpolator->get(&tmp.targetRpy(0), true); + } + // pitch + if (use_pitch_flywheel || gg->is_jumping) { + if (!prev_pitch_state) { + if (pitch_weight_interpolator->isEmpty()) pitch_weight = normal_weight; + else pitch_weight_interpolator->get(&pitch_weight, true); + pitch_weight_interpolator->set(&pitch_weight); + pitch_weight_interpolator->setGoal(&fly_weight, weight_fly_interpolator_time, true); + } + if (pitch_weight_interpolator->isEmpty()) pitch_weight = fly_weight; + else pitch_weight_interpolator->get(&pitch_weight, true); + } else { + if (prev_pitch_state) { + if (pitch_weight_interpolator->isEmpty()) pitch_weight = fly_weight; + else pitch_weight_interpolator->get(&pitch_weight, true); + pitch_weight_interpolator->set(&pitch_weight); + pitch_weight_interpolator->setGoal(&normal_weight, weight_normal_interpolator_time, true); + double zero_momentum = 0.0; + pitch_momentum_interpolator->set(&prev_momentum(1)); + pitch_momentum_interpolator->setGoal(&zero_momentum, weight_normal_interpolator_time, true); + } + if (pitch_weight_interpolator->isEmpty()) pitch_weight = normal_weight; + else pitch_weight_interpolator->get(&pitch_weight, true); + if (!pitch_momentum_interpolator->isEmpty()) pitch_momentum_interpolator->get(&tmp.targetRpy(1), true); + } + if (!cog_constraint_interpolator->isEmpty()) { + cog_constraint_interpolator->get(&cog_z_constraint, true); + } + tmp.constraint_weight << 1,1,cog_z_constraint,roll_weight*transition_interpolator_ratio,pitch_weight*transition_interpolator_ratio,0; // consider angular momentum (COMMON) + + // 上半身関節角のq_refへの緩い拘束 + double upper_weight, fly_ratio = 1e-10, normal_ratio = 2e-6; + if (is_natural_walk) normal_ratio = 1e-5; + if (ikp.size() >= 4 && (ikp["rarm"].is_active || ikp["larm"].is_active)) normal_ratio = 1e-6; + if (use_roll_flywheel || use_pitch_flywheel) { + if (!prev_roll_state && !prev_pitch_state) { + if (angular_momentum_interpolator->isEmpty()) upper_weight = normal_ratio; + else angular_momentum_interpolator->get(&upper_weight, true); + angular_momentum_interpolator->set(&upper_weight); + angular_momentum_interpolator->setGoal(&fly_ratio, weight_fly_interpolator_time, true); + } + if (angular_momentum_interpolator->isEmpty()) upper_weight = fly_ratio; + else angular_momentum_interpolator->get(&upper_weight, true); + } else { + if (prev_roll_state || prev_pitch_state) { + if (angular_momentum_interpolator->isEmpty()) upper_weight = fly_ratio; + else angular_momentum_interpolator->get(&upper_weight, true); + angular_momentum_interpolator->set(&upper_weight); + angular_momentum_interpolator->setGoal(&normal_ratio, weight_normal_interpolator_time, true); + } + if (angular_momentum_interpolator->isEmpty()) upper_weight = normal_ratio; + else angular_momentum_interpolator->get(&upper_weight, true); + } + fik->q_ref_constraint_weight.head(m_robot->numJoints()).fill(upper_weight); // except for rootLink + + prev_roll_state = use_roll_flywheel || gg->is_jumping; + prev_pitch_state = use_pitch_flywheel || gg->is_jumping; + fik->q_ref_constraint_weight.segment(fik->ikp["rleg"].group_indices.back(), fik->ikp["rleg"].group_indices.size()).fill(0); + fik->q_ref_constraint_weight.segment(fik->ikp["lleg"].group_indices.back(), fik->ikp["lleg"].group_indices.size()).fill(0); + if(m_robot->link("RARM_F_JOINT0") != NULL) fik->q_ref_constraint_weight[m_robot->link("RARM_F_JOINT0")->jointId] = 1e-3; + if(m_robot->link("RARM_F_JOINT1") != NULL) fik->q_ref_constraint_weight[m_robot->link("RARM_F_JOINT1")->jointId] = 1e-3; + if(m_robot->link("LARM_F_JOINT0") != NULL) fik->q_ref_constraint_weight[m_robot->link("LARM_F_JOINT0")->jointId] = 1e-3; + if(m_robot->link("LARM_F_JOINT1") != NULL) fik->q_ref_constraint_weight[m_robot->link("LARM_F_JOINT1")->jointId] = 1e-3; + if(m_robot->link("HEAD_JOINT0") != NULL) fik->q_ref_constraint_weight[m_robot->link("HEAD_JOINT0")->jointId] = 1e-3; + if(m_robot->link("HEAD_JOINT1") != NULL) fik->q_ref_constraint_weight[m_robot->link("HEAD_JOINT1")->jointId] = 1e-3; + +// tmp.rot_precision = 1e-1;//angular momentum precision + ik_tgt_list.push_back(tmp); + } + // knee stretch protection + if(m_robot->link("RLEG_JOINT3") != NULL) m_robot->link("RLEG_JOINT3")->llimit = deg2rad(5); + if(m_robot->link("LLEG_JOINT3") != NULL) m_robot->link("LLEG_JOINT3")->llimit = deg2rad(5); + if(m_robot->link("R_KNEE_P") != NULL) m_robot->link("R_KNEE_P")->llimit = deg2rad(5); + if(m_robot->link("L_KNEE_P") != NULL) m_robot->link("L_KNEE_P")->llimit = deg2rad(5); + // reduce chest joint move + if(m_robot->link("HEAD_JOINT0") != NULL) fik->dq_weight_all(m_robot->link("HEAD_JOINT0")->jointId) = 10; + if(m_robot->link("HEAD_JOINT1") != NULL) fik->dq_weight_all(m_robot->link("HEAD_JOINT1")->jointId) = 10; + // reduce chest joint move + if(m_robot->link("CHEST_JOINT0") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT0")->jointId) = 10; + if(m_robot->link("CHEST_JOINT1") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT1")->jointId) = 10; + if(m_robot->link("CHEST_JOINT2") != NULL) fik->dq_weight_all(m_robot->link("CHEST_JOINT2")->jointId) = 10; + if(m_robot->link("CHEST_Y") != NULL) fik->dq_weight_all(m_robot->link("CHEST_Y")->jointId) = 10; + if(m_robot->link("CHEST_P") != NULL) fik->dq_weight_all(m_robot->link("CHEST_P")->jointId) = 10; + // disable toe joint move + if(m_robot->link("RLEG_JOINT6") != NULL) fik->dq_weight_all(m_robot->link("RLEG_JOINT6")->jointId) = 1e6; + if(m_robot->link("LLEG_JOINT6") != NULL) fik->dq_weight_all(m_robot->link("LLEG_JOINT6")->jointId) = 1e6; + // fik->dq_weight_all.tail(3).fill(1e1);//ベースリンク回転変位の重みは1e1以下は暴れる？ + fik->rootlink_rpy_llimit << deg2rad(-15), deg2rad(-30), -DBL_MAX; + fik->rootlink_rpy_ulimit << deg2rad(15), deg2rad(45), DBL_MAX; + + int loop_result = 0; + loop_result = fik->solveFullbodyIKLoop(m_robot, ik_tgt_list, ik_max_loop); +} + +void AutoBalancer::solveSimpleFullbodyIK () +{ + // Set ik target params + fik->target_root_p = target_root_p; + fik->target_root_R = target_root_R; + for ( std::map::iterator it = fik->ikp.begin(); it != fik->ikp.end(); it++ ) { + it->second.target_p0 = ikp[it->first].target_p0; + it->second.target_r0 = ikp[it->first].target_r0; + } + fik->ratio_for_vel = transition_interpolator_ratio * leg_names_interpolator_ratio; +// fik->current_tm = m_qRef.tm; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + fik->ikp[it->first].is_ik_enable = it->second.is_active; + } + // Revert + fik->revertRobotStateToCurrent(); + hrp::Vector3 tmp_input_sbp = hrp::Vector3(0,0,0); + static_balance_point_proc_one(tmp_input_sbp, ref_zmp(2)); + hrp::Vector3 dif_cog = tmp_input_sbp - ref_cog; + + // Solve IK + fik->solveFullbodyIK (dif_cog, transition_interpolator->isEmpty()); + for (size_t i = 0; i < 2; i++) { // ik loop + dif_cog = m_robot->calcCM() - ref_cog - dif_ref_act_cog; + fik->solveSimpleFullbodyIKLoop(dif_cog, transition_interpolator->isEmpty()); + } +} + +void AutoBalancer::calcTotalExternalForceZ () +{ + total_external_force_z = 0.0; + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + size_t idx = contact_states_index_map[it->first]; + if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) { // Not included in leg_names + total_external_force_z += m_ref_force[idx].data[2]; + } + } + if ( use_force == MODE_ADDITIONAL_FORCE ) { + size_t idx = ref_forces.size() - 1; + total_external_force_z += m_ref_force[idx].data[2]; + } + gg->total_external_force_z = total_external_force_z; + st->total_external_force_z = total_external_force_z; +} + +void AutoBalancer::fixActCogVelForWheel () +{ + double wheel_base_vel = 0.0; + int contact_cnt = 0; + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { + if (std::find(leg_names.begin(), leg_names.end(), it->first) != leg_names.end()) { + const double cur_pos = st->qCurrent[wheel_id[it->first]] * wheel_radius; + size_t idx = contact_states_index_map[it->first]; + if (m_contactStates.data[idx]) { + wheel_base_vel += (cur_pos - prev_wheel_pos[it->first]) / m_dt; + contact_cnt++; } + prev_wheel_pos[it->first] = cur_pos; + } } - m_robot->calcForwardKinematics(); + if (gg->is_wheeling) { + st->act_cogvel(0) += wheel_base_vel / static_cast(contact_cnt); + } +} - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.is_active) solveLimbIKforLimb(it->second); +void AutoBalancer::limit_cog (hrp::Vector3& cog) +{ + if (transition_interpolator->isEmpty() && !gg_is_walking && is_after_walking && !gg->is_jumping && !gg->is_wheeling ) { + std::vector start_pos(3), start_vel(3), goal_pos(3); + double tmp_time = 5.0; + for (size_t i = 0; i < 3; i++) { + start_pos[i] = orig_cog(i); + start_vel[i] = (orig_cog(i) - prev_orig_cog(i)) / m_dt; + goal_pos[i] = cog(i); + } + limit_cog_interpolator->set(start_pos.data(), start_vel.data()); + limit_cog_interpolator->setGoal(goal_pos.data(), tmp_time, true); + is_after_walking = false; + } + if (!limit_cog_interpolator->isEmpty()) { + std::vector tmp_v(3); + double tmp_time = limit_cog_interpolator->get_remain_time(); + for (size_t i = 0; i < 3; i++) { + tmp_v[i] = cog(i); + } + limit_cog_interpolator->setGoal(tmp_v.data(), tmp_time, true); + limit_cog_interpolator->get(tmp_v.data(), true); + for (size_t i = 0; i < 3; i++) { + cog(i) = tmp_v[i]; + } } - if (gg_is_walking && !gg_solved) stopWalking (); } +bool AutoBalancer::vlimit(double& ret, const double llimit_value, const double ulimit_value) +{ + if (ret > ulimit_value) { + ret = ulimit_value; + return false; + } else if (ret < llimit_value) { + ret = llimit_value; + return false; + } + return true; +}; + + /* RTC::ReturnCode_t AutoBalancer::onAborting(RTC::UniqueId ec_id) { @@ -1023,10 +2245,28 @@ void AutoBalancer::startABCparam(const OpenHRP::AutoBalancerService::StrSequence transition_interpolator->clear(); transition_interpolator->set(&tmp_ratio); tmp_ratio = 1.0; - transition_interpolator->go(&tmp_ratio, transition_time, true); + transition_interpolator->setGoal(&tmp_ratio, transition_time, true); + prev_ref_zmp = ref_zmp; + prev_roll_state = false; + prev_pitch_state = false; + prev_orig_dif_p = hrp::Vector3::Zero(); + gg->set_is_emergency_step(false); + d_wheel_angle = 0.0; + start_d_wheel_angle = 0.0; + angular_momentum_interpolator->clear(); + roll_weight_interpolator->clear(); + pitch_weight_interpolator->clear(); + roll_momentum_interpolator->clear(); + pitch_momentum_interpolator->clear(); + limit_cog_interpolator->clear(); + fik->resetCollision(); + hand_fix_interpolator->clear(); for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { it->second.is_active = false; } + for (size_t i = 0; i < 2; i++) { + touchdown_transition_interpolator[leg_names[i]]->clear(); + } for (size_t i = 0; i < limbs.length(); i++) { ABCIKparam& tmp = ikp[std::string(limbs[i])]; @@ -1045,27 +2285,47 @@ void AutoBalancer::stopABCparam() transition_interpolator->clear(); transition_interpolator->set(&tmp_ratio); tmp_ratio = 0.0; - transition_interpolator->go(&tmp_ratio, transition_time, true); + transition_interpolator->setGoal(&tmp_ratio, transition_time, true); control_mode = MODE_SYNC_TO_IDLE; } -void AutoBalancer::startWalking () +void AutoBalancer::stopABCparamEmergency() +{ + std::cerr << "[" << m_profile.instance_name << "] stop auto balancer mode for emergency" << std::endl; + double tmp_ratio = 1.0, tmp_time = (gg->is_emergency_touch_wall && is_emergency_touch_wall_mode) ? 0.8 : 0.8; + emergency_transition_interpolator->clear(); + emergency_transition_interpolator->set(&tmp_ratio); + tmp_ratio = 0.0; + emergency_transition_interpolator->setGoal(&tmp_ratio, tmp_time, true); + for (int i = 0; i < m_robot->numJoints(); i++ ) { + diff_q[i] = m_robot->joint(i)->q - m_qRef.data[i]; + } + control_mode = MODE_IDLE; + gg->clear_footstep_nodes_list(); + for ( std::map::iterator it = touchdown_transition_interpolator.begin(); it != touchdown_transition_interpolator.end(); it++ ) { + it->second->clear(); + } + gg_is_walking = false; + gg->is_jumping = false; + gg->is_wheeling = false; +} + +bool AutoBalancer::startWalking (const bool is_wheel) { if ( control_mode != MODE_ABC ) { - return_control_mode = control_mode; - OpenHRP::AutoBalancerService::StrSequence fix_limbs; - fix_limbs.length(2); - fix_limbs[0] = "rleg"; - fix_limbs[1] = "lleg"; - startABCparam(fix_limbs); - waitABCTransition(); + std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ABC. Please startAutoBalancer." << std::endl; + return false; + } + if ( gg->use_act_states && st->control_mode != Stabilizer::MODE_ST ) { + std::cerr << "[" << m_profile.instance_name << "] Cannot start walking without MODE_ST. Please startStabilizer." << std::endl; + return false; } + hrp::Vector3 act_cog = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cog; + act_cog.head(2) += sbp_cog_offset.head(2); + hrp::Vector3 act_cogvel = st->ref_foot_origin_rot * st->act_cogvel; { Guard guard(m_mutex); - has_ik_failed = false; - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0; - } + fik->resetIKFailParam(); std::vector init_swing_leg_names(gg->get_footstep_front_leg_names()); std::vector tmp_all_limbs(leg_names); std::vector init_support_leg_names; @@ -1076,43 +2336,98 @@ void AutoBalancer::startWalking () std::back_inserter(init_support_leg_names)); std::vector init_support_leg_steps, init_swing_leg_dst_steps; for (std::vector::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++) - init_support_leg_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0)); + init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0)); for (std::vector::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++) - init_swing_leg_dst_steps.push_back(step_node(*it, ikp[*it].target_end_coords, 0, 0, 0, 0)); + init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0)); gg->set_default_zmp_offsets(default_zmp_offsets); - gg->initialize_gait_parameter(ref_cog, init_support_leg_steps, init_swing_leg_dst_steps); + gg->initialize_gait_parameter(act_cog, ref_cog, init_support_leg_steps, init_swing_leg_dst_steps); + if (is_wheel) gg->initialize_wheel_parameter(act_cog, ref_cog, init_support_leg_steps, init_swing_leg_dst_steps); } is_hand_fix_initial = true; - while ( !gg->proc_one_tick() ); + while ( !gg->proc_one_tick(act_cog, act_cogvel, target_cog) ); { Guard guard(m_mutex); gg_is_walking = gg_solved = true; + if (is_wheel) gg->is_wheeling = true; + is_after_walking = true; + limit_cog_interpolator->clear(); } + return true; } void AutoBalancer::stopWalking () { std::vector tmp_end_coords_list; for (std::vector::iterator it = leg_names.begin(); it != leg_names.end(); it++) { - if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(ikp[*it].target_end_coords); + if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0)); } multi_mid_coords(fix_leg_coords, tmp_end_coords_list); fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot); gg->clear_footstep_nodes_list(); - if (return_control_mode == MODE_IDLE) stopABCparam(); gg_is_walking = false; } +void AutoBalancer::stopJumping () +{ + std::vector tmp_end_coords_list; + for (std::vector::iterator it = leg_names.begin(); it != leg_names.end(); it++) { + if ((*it).find("leg") != std::string::npos) tmp_end_coords_list.push_back(coordinates(ikp[*it].target_p0, ikp[*it].target_r0)); + } + multi_mid_coords(fix_leg_coords, tmp_end_coords_list); + fixLegToCoords(fix_leg_coords.pos, fix_leg_coords.rot); +} + +void AutoBalancer::stopWheeling () +{ + stopJumping(); +} + bool AutoBalancer::startAutoBalancer (const OpenHRP::AutoBalancerService::StrSequence& limbs) { if (control_mode == MODE_IDLE) { - has_ik_failed = false; - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0; + if (gg_is_walking) { + // std::cerr << "[" << m_profile.instance_name << "] goStop before startAutoBalancer because gg_is_walking is true" << std::endl; + // goStop(); + std::cerr << "[" << m_profile.instance_name << "] Cannot startAutobalancer while gg_is_walking is true. Please goStop or wait for ending previous goPos." << std::endl; + return false; } + fik->resetIKFailParam(); startABCparam(limbs); waitABCTransition(); - return_control_mode = MODE_ABC; + return true; + } else { + return false; + } +} + +bool AutoBalancer::jumpTo(const double& x, const double& y, const double& z, const double& ts, const double& tf) +{ + if (!gg->is_jumping && !gg_is_walking) { + gg->set_all_limbs(leg_names); + coordinates start_ref_coords; + std::vector initial_support_legs_coords; // dummy + std::vector initial_support_legs; // dummy + bool is_valid_gait_type = calc_inital_support_legs(0, initial_support_legs_coords, initial_support_legs, start_ref_coords); + if (is_valid_gait_type == false) return false; + + // initialize jump generation + hrp::Vector3 act_cog = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cog; + act_cog.head(2) += sbp_cog_offset.head(2); + + std::vector init_swing_leg_names(1, "rleg"); + std::vector init_support_leg_names(1, "lleg"); + std::vector init_support_leg_steps, init_swing_leg_dst_steps; + for (std::vector::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++) + init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0)); + for (std::vector::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++) + init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0)); + gg->set_default_zmp_offsets(default_zmp_offsets); + gg->initialize_jump_parameter(act_cog, ref_cog, init_support_leg_steps, init_swing_leg_dst_steps, start_ref_coords, hrp::Vector3(x, y, z), ts, tf); + gg->is_jumping = true; + + is_after_walking = true; + limit_cog_interpolator->clear(); + return true; } else { return false; @@ -1130,6 +2445,16 @@ bool AutoBalancer::stopAutoBalancer () } } +void AutoBalancer::startStabilizer(void) +{ + st->startStabilizer(); +} + +void AutoBalancer::stopStabilizer(void) +{ + st->stopStabilizer(); +} + void AutoBalancer::waitABCTransition() { while (!transition_interpolator->isEmpty()) usleep(1000); @@ -1145,60 +2470,167 @@ bool AutoBalancer::goPos(const double& x, const double& y, const double& th) std::vector initial_support_legs; bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords); if (is_valid_gait_type == false) return false; + gg->set_vel_foot_offset(start_ref_coords.rot.transpose() * (ikp["rleg"].target_p0 - start_ref_coords.pos), RLEG); + gg->set_vel_foot_offset(start_ref_coords.rot.transpose() * (ikp["lleg"].target_p0 - start_ref_coords.pos), LLEG); + bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th, + initial_support_legs_coords, // Dummy if gg_is_walking + start_ref_coords, // Dummy if gg_is_walking + initial_support_legs, // Dummy if gg_is_walking + (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps. + if (!ret) { + std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl; + } + if ( !gg_is_walking ) { // Initializing + ret = startWalking(); + } + return ret; + } else { + std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl; + return false; + } +} +bool AutoBalancer::goPosWheel(const double& x, const double& y, const double& th, const double w_x, const double rv_max, const double ra_max, const double tm_off) +{ + if ( !gg->is_wheeling && !is_stop_mode) { + gg->set_all_limbs(leg_names); + coordinates start_ref_coords; + std::vector initial_support_legs_coords; + std::vector initial_support_legs; + bool is_valid_gait_type = calc_inital_support_legs(y, initial_support_legs_coords, initial_support_legs, start_ref_coords); + if (is_valid_gait_type == false) return false; + gg->set_vel_foot_offset(start_ref_coords.rot.transpose() * (ikp["rleg"].target_p0 - start_ref_coords.pos), RLEG); + gg->set_vel_foot_offset(start_ref_coords.rot.transpose() * (ikp["lleg"].target_p0 - start_ref_coords.pos), LLEG); bool ret = gg->go_pos_param_2_footstep_nodes_list(x, y, th, initial_support_legs_coords, // Dummy if gg_is_walking start_ref_coords, // Dummy if gg_is_walking initial_support_legs, // Dummy if gg_is_walking - (!gg_is_walking)); // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps. - if ( !gg_is_walking ) { // Initializing - startWalking(); - } + (!gg_is_walking), // If gg_is_walking, initialize. Otherwise, not initialize and overwrite footsteps. + tm_off); if (!ret) { std::cerr << "[" << m_profile.instance_name << "] Cannot goPos because of invalid timing." << std::endl; } + if ( !gg_is_walking ) { // Initializing + const std::vector x_list(1, w_x); + const std::vector v_max_list(1, deg2rad(rv_max)*wheel_radius); + const std::vector a_max_list(1, deg2rad(ra_max)*wheel_radius); + ret = gg->go_wheel_param_2_wheel_nodes_list(x_list, v_max_list, a_max_list, start_ref_coords, true); + ret &= startWalking(true); + } return ret; } else { std::cerr << "[" << m_profile.instance_name << "] Cannot goPos while stopping mode." << std::endl; return false; } } +bool AutoBalancer::goWheel(const double& x, const double& rv_max, const double& ra_max) +{ + // initialize wheel node + if (!gg->is_wheeling) { + gg->set_all_limbs(leg_names); + coordinates start_ref_coords; + std::vector initial_support_legs_coords; // dummy + std::vector initial_support_legs; // dummy + bool is_valid_gait_type = calc_inital_support_legs(0, initial_support_legs_coords, initial_support_legs, start_ref_coords); + if (is_valid_gait_type == false) return false; + // gg->set_vel_foot_offset(start_ref_coords.rot.transpose() * (ikp["rleg"].target_p0 - start_ref_coords.pos), RLEG); + // gg->set_vel_foot_offset(start_ref_coords.rot.transpose() * (ikp["lleg"].target_p0 - start_ref_coords.pos), LLEG); + bool ret = false; + { + const std::vector x_list(1, x); + const std::vector v_max_list(1, deg2rad(rv_max)*wheel_radius); + const std::vector a_max_list(1, deg2rad(ra_max)*wheel_radius); + ret = gg->go_wheel_param_2_wheel_nodes_list(x_list, v_max_list, a_max_list, start_ref_coords, false); + } + ret &= startWheeling(); + return ret; + } else { + std::cerr << "[" << m_profile.instance_name << "] Cannot goWheel while gg_is_wheeling." << std::endl; + return false; + } +} + +bool AutoBalancer::startWheeling() +{ + // initialize wheel generation + hrp::Vector3 act_cog = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cog; + act_cog.head(2) += sbp_cog_offset.head(2); + + std::vector init_swing_leg_names(1, "rleg"); + std::vector init_support_leg_names(1, "lleg"); + std::vector init_support_leg_steps, init_swing_leg_dst_steps; + for (std::vector::iterator it = init_support_leg_names.begin(); it != init_support_leg_names.end(); it++) + init_support_leg_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0)); + for (std::vector::iterator it = init_swing_leg_names.begin(); it != init_swing_leg_names.end(); it++) + init_swing_leg_dst_steps.push_back(step_node(*it, coordinates(ikp[*it].target_p0, ikp[*it].target_r0), 0, 0, 0, 0)); + gg->set_default_zmp_offsets(default_zmp_offsets); + gg->initialize_wheel_parameter(act_cog, ref_cog, init_support_leg_steps, init_swing_leg_dst_steps); + gg->is_wheeling = true; + + is_after_walking = true; + limit_cog_interpolator->clear(); + + return true; +} bool AutoBalancer::goVelocity(const double& vx, const double& vy, const double& vth) { - gg->set_all_limbs(leg_names); - if (gg_is_walking && gg_solved) { - gg->set_velocity_param(vx, vy, vth); - } else { - coordinates ref_coords; - mid_coords(ref_coords, 0.5, ikp["rleg"].target_end_coords, ikp["lleg"].target_end_coords); - std::vector current_legs; - switch(gait_type) { - case BIPED: - current_legs = (vy > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG)); + if (!is_stop_mode) { + gg->set_all_limbs(leg_names); + bool ret = true; + if (gg_is_walking && gg_solved) { + gg->set_velocity_param(vx, vy, vth); + // std::vector tmp_v(3), target_v(3); + // target_v[0] = vx; + // target_v[1] = vy; + // target_v[2] = vth; + // double transition_time = 5.0; + // if (go_vel_interpolator->isEmpty()) { + // go_vel_interpolator->clear(); + // go_vel_interpolator->setGoal(target_v.data(), transition_time, true); + // } else { + // go_vel_interpolator->setGoal(target_v.data(), transition_time, true); + // } + // go_vel_interpolator->get(tmp_v.data(), true); + // gg->set_velocity_param(tmp_v[0], tmp_v[1], tmp_v[2]); + } else { + coordinates ref_coords; + ref_coords.pos = (ikp["rleg"].target_p0+ikp["lleg"].target_p0)*0.5; + mid_rot(ref_coords.rot, 0.5, ikp["rleg"].target_r0, ikp["lleg"].target_r0); + std::vector current_legs; + switch(gait_type) { + case BIPED: + current_legs.assign (1, vy > 0 ? RLEG : LLEG); break; - case TROT: - current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)); + case TROT: + current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)).convert_to_container < std::vector > (); break; - case PACE: - current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)); + case PACE: + current_legs = (vy > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)).convert_to_container < std::vector > (); break; - case CRAWL: + case CRAWL: std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl; return false; - case GALLOP: + case GALLOP: /* at least one leg shoud be in contact */ std::cerr << "[" << m_profile.instance_name << "] gallop walk[" << gait_type << "] is not implemented yet." << std::endl; return false; - default: break; + default: break; + } + gg->set_vel_foot_offset(ref_coords.rot.transpose() * (ikp["rleg"].target_p0 - ref_coords.pos), RLEG); + gg->set_vel_foot_offset(ref_coords.rot.transpose() * (ikp["lleg"].target_p0 - ref_coords.pos), LLEG); + gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs); + ret = startWalking(); } - gg->initialize_velocity_mode(ref_coords, vx, vy, vth, current_legs); - startWalking(); + return ret; + } else { + std::cerr << "[" << m_profile.instance_name << "] Cannot goVelocity while stopping mode." << std::endl; + return false; } - return true; } bool AutoBalancer::goStop () { + go_vel_interpolator->clear(); gg->finalize_velocity_mode(); waitFootSteps(); return true; @@ -1237,10 +2669,10 @@ bool AutoBalancer::setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSeq spss[i].sps[j].heel_angle = ((!gg_is_walking && i==0) ? 0.0 : gg->get_heel_angle()); } } - setFootStepsWithParam(fss, spss, overwrite_fs_idx); + return setFootStepsWithParam(fss, spss, overwrite_fs_idx); } -bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx) +bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx, const bool is_wheel) { if (!is_stop_mode) { std::cerr << "[" << m_profile.instance_name << "] setFootStepsList" << std::endl; @@ -1262,7 +2694,8 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo } else { // If walking, snap initial leg to current ABC foot coords. for (size_t i = 0; i < fss[0].fs.length(); i++) { - initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), ikp[std::string(fss[0].fs[i].leg)].target_end_coords, 0, 0, 0, 0)); + ABCIKparam& tmpikp = ikp[std::string(fss[0].fs[i].leg)]; + initial_support_steps.push_back(step_node(std::string(fss[0].fs[i].leg), coordinates(tmpikp.target_p0, tmpikp.target_r0), 0, 0, 0, 0)); } } initial_support_step = initial_support_steps.front(); /* use only one leg for representation */ @@ -1318,6 +2751,7 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo fnsl.push_back(tmp_fns); } } + bool ret = true; if (gg_is_walking) { std::cerr << "[" << m_profile.instance_name << "] Set overwrite footsteps" << std::endl; gg->set_overwrite_foot_steps_list(fnsl); @@ -1325,19 +2759,45 @@ bool AutoBalancer::setFootStepsWithParam(const OpenHRP::AutoBalancerService::Foo } else { std::cerr << "[" << m_profile.instance_name << "] Set normal footsteps" << std::endl; gg->set_foot_steps_list(fnsl); - startWalking(); + ret = startWalking(is_wheel); } - return true; + return ret; } else { std::cerr << "[" << m_profile.instance_name << "] Cannot setFootSteps while walking." << std::endl; return false; } } +bool AutoBalancer::setFootStepsWithWheel(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, const OpenHRP::AutoBalancerService::WheelParamsSequence& wpss, CORBA::Long overwrite_fs_idx) +{ + bool ret = false; + bool with_footstep = (fss.length() > 1); + { + coordinates start_ref_coords; + std::vector initial_support_legs_coords; // dummy + std::vector initial_support_legs; // dummy + bool is_valid_gait_type = calc_inital_support_legs(0, initial_support_legs_coords, initial_support_legs, start_ref_coords); + const size_t wpss_len = wpss.length(); + std::vector x_list(wpss_len); + std::vector v_max_list(wpss_len); + std::vector a_max_list(wpss_len); + for (size_t i = 0; i < wpss_len; i++) { + x_list[i] = wpss[i].wps[0].goal_pos; + v_max_list[i] = deg2rad(wpss[i].wps[0].rv_max)*wheel_radius; + a_max_list[i] = deg2rad(wpss[i].wps[0].ra_max)*wheel_radius; + } + ret = gg->go_wheel_param_2_wheel_nodes_list(x_list, v_max_list, a_max_list, start_ref_coords, with_footstep); + } + if (with_footstep) ret &= setFootStepsWithParam(fss, spss, overwrite_fs_idx, true); + else ret &= startWheeling(); + + return ret; +} + void AutoBalancer::waitFootSteps() { //while (gg_is_walking) usleep(10); - while (gg_is_walking || !transition_interpolator->isEmpty() ) + while (gg_is_walking || !transition_interpolator->isEmpty() || gg->is_jumping || gg->is_wheeling) usleep(1000); usleep(1000); gg->set_offset_velocity_param(0,0,0); @@ -1355,7 +2815,13 @@ void AutoBalancer::waitFootStepsEarly(const double tm) bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param) { std::cerr << "[" << m_profile.instance_name << "] setGaitGeneratorParam" << std::endl; - gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]); + if (i_param.stride_parameter.length() == 4) { // Support old stride_parameter definitions + gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], + i_param.stride_parameter[1]*0.5, i_param.stride_parameter[2]*0.5); + } else { + gg->set_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], + i_param.stride_parameter[4], i_param.stride_parameter[5]); + } std::vector off; for (size_t i = 0; i < i_param.leg_default_translate_pos.length(); i++) { off.push_back(hrp::Vector3(i_param.leg_default_translate_pos[i][0], i_param.leg_default_translate_pos[i][1], i_param.leg_default_translate_pos[i][2])); @@ -1394,8 +2860,11 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai } gg->set_swing_trajectory_delay_time_offset(i_param.swing_trajectory_delay_time_offset); gg->set_swing_trajectory_final_distance_weight(i_param.swing_trajectory_final_distance_weight); + gg->set_swing_trajectory_time_offset_xy2z(i_param.swing_trajectory_time_offset_xy2z); gg->set_stair_trajectory_way_point_offset(hrp::Vector3(i_param.stair_trajectory_way_point_offset[0], i_param.stair_trajectory_way_point_offset[1], i_param.stair_trajectory_way_point_offset[2])); - gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2])); + gg->set_rectangle_trajectory_way_point_offset(hrp::Vector3(i_param.rectangle_trajectory_way_point_offset[0], i_param.rectangle_trajectory_way_point_offset[1], i_param.rectangle_trajectory_way_point_offset[2])); + gg->set_rectangle_goal_off(hrp::Vector3(i_param.rectangle_goal_off[0], i_param.rectangle_goal_off[1], i_param.rectangle_goal_off[2])); + gg->set_cycloid_delay_kick_point_offset(hrp::Vector3(i_param.cycloid_delay_kick_point_offset[0], i_param.cycloid_delay_kick_point_offset[1], i_param.cycloid_delay_kick_point_offset[2])); gg->set_gravitational_acceleration(i_param.gravitational_acceleration); gg->set_toe_angle(i_param.toe_angle); gg->set_heel_angle(i_param.heel_angle); @@ -1403,14 +2872,66 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai gg->set_heel_pos_offset_x(i_param.heel_pos_offset_x); gg->set_toe_zmp_offset_x(i_param.toe_zmp_offset_x); gg->set_heel_zmp_offset_x(i_param.heel_zmp_offset_x); + gg->set_toe_check_thre(i_param.toe_check_thre); + gg->set_heel_check_thre(i_param.heel_check_thre); std::vector tmp_ratio(i_param.toe_heel_phase_ratio.get_buffer(), i_param.toe_heel_phase_ratio.get_buffer()+i_param.toe_heel_phase_ratio.length()); std::cerr << "[" << m_profile.instance_name << "] "; // for set_toe_heel_phase_ratio gg->set_toe_heel_phase_ratio(tmp_ratio); gg->set_use_toe_joint(i_param.use_toe_joint); gg->set_use_toe_heel_transition(i_param.use_toe_heel_transition); + gg->set_use_toe_heel_auto_set(i_param.use_toe_heel_auto_set); gg->set_zmp_weight_map(boost::assign::map_list_of(RLEG, i_param.zmp_weight_map[0])(LLEG, i_param.zmp_weight_map[1])(RARM, i_param.zmp_weight_map[2])(LARM, i_param.zmp_weight_map[3])); gg->set_optional_go_pos_finalize_footstep_num(i_param.optional_go_pos_finalize_footstep_num); - gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset); + if (i_param.overwritable_footstep_index_offset < 4) { + gg->set_overwritable_footstep_index_offset(i_param.overwritable_footstep_index_offset); + } else { + std::cerr << "[" << m_profile.instance_name << "] overwritable_footstep_index_offset must be less than 4" << std::endl; + } + gg->set_leg_margin(i_param.leg_margin); + gg->set_safe_leg_margin(i_param.safe_leg_margin); + gg->set_vertices_from_leg_margin(); + gg->set_stride_limitation_for_circle_type(i_param.stride_limitation_for_circle_type); + gg->set_overwritable_stride_limitation(i_param.overwritable_stride_limitation); + gg->set_use_stride_limitation(i_param.use_stride_limitation); + gg->set_footstep_modification_gain(i_param.footstep_modification_gain); + gg->set_modify_footsteps(i_param.modify_footsteps); + gg->set_min_time_mgn(i_param.min_time_mgn); + gg->set_min_time(i_param.min_time); + gg->set_cp_check_margin(i_param.cp_check_margin); + gg->set_margin_time_ratio(i_param.margin_time_ratio); + if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::SQUARE) { + gg->set_stride_limitation_type(SQUARE); + } else if (i_param.stride_limitation_type == OpenHRP::AutoBalancerService::CIRCLE) { + gg->set_stride_limitation_type(CIRCLE); + } + gg->set_act_vel_ratio(i_param.act_vel_ratio); + gg->set_use_disturbance_compensation(i_param.use_disturbance_compensation); + gg->set_dc_gain(i_param.dc_gain); + gg->set_dcm_offset(i_param.dcm_offset); + gg->set_emergency_step_time(i_param.emergency_step_time); + gg->use_act_states = st->use_act_states = i_param.use_act_states; + gg->set_use_act_states(); + gg->is_interpolate_zmp_in_double = i_param.is_interpolate_zmp_in_double; + gg->is_stable_go_stop_mode = i_param.is_stable_go_stop_mode; + gg->use_flywheel_balance = i_param.use_flywheel_balance; + gg->zmp_delay_time_const = i_param.zmp_delay_time_const; + gg->overwritable_max_time = i_param.overwritable_max_time; + gg->fg_zmp_cutoff_freq = i_param.fg_zmp_cutoff_freq; + gg->cp_filter->setCutOffFreq(i_param.fg_cp_cutoff_freq); + gg->set_sum_d_footstep_thre(hrp::Vector3(i_param.sum_d_footstep_thre[0], i_param.sum_d_footstep_thre[1], i_param.sum_d_footstep_thre[2])); + gg->set_footstep_check_delta(hrp::Vector3(i_param.footstep_check_delta[0], i_param.footstep_check_delta[1], i_param.footstep_check_delta[2])); + gg->debug_set_landing_height = i_param.debug_set_landing_height; + gg->debug_landing_height = i_param.debug_landing_height; + for (size_t i = 0; i < 2; i++) { + gg->debug_landing_height_xrange[i] = i_param.debug_landing_height_xrange[i]; + } + for (size_t i = 0; i < 2; i++) { + gg->front_edge_offset_of_steppable_region[i] = i_param.front_edge_offset_of_steppable_region[i]; + } + gg->is_slow_stair_mode = i_param.is_slow_stair_mode; + gg->stair_step_time = i_param.stair_step_time; + gg->num_preview_step = i_param.num_preview_step; + gg->set_rectangle_time_smooth_offset(i_param.rectangle_time_smooth_offset); // print gg->print_param(std::string(m_profile.instance_name)); @@ -1419,7 +2940,7 @@ bool AutoBalancer::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::Gai bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param) { - gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3]); + gg->get_stride_parameters(i_param.stride_parameter[0], i_param.stride_parameter[1], i_param.stride_parameter[2], i_param.stride_parameter[3], i_param.stride_parameter[4], i_param.stride_parameter[5]); std::vector off; gg->get_leg_default_translate_pos(off); i_param.leg_default_translate_pos.length(off.size()); @@ -1457,10 +2978,15 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener hrp::Vector3 tmpv = gg->get_stair_trajectory_way_point_offset(); for (size_t i = 0; i < 3; i++) i_param.stair_trajectory_way_point_offset[i] = tmpv(i); + tmpv = gg->get_rectangle_trajectory_way_point_offset(); + for (size_t i = 0; i < 3; i++) i_param.rectangle_trajectory_way_point_offset[i] = tmpv(i); + tmpv = gg->get_rectangle_goal_off(); + for (size_t i = 0; i < 3; i++) i_param.rectangle_goal_off[i] = tmpv(i); tmpv = gg->get_cycloid_delay_kick_point_offset(); for (size_t i = 0; i < 3; i++) i_param.cycloid_delay_kick_point_offset[i] = tmpv(i); i_param.swing_trajectory_delay_time_offset = gg->get_swing_trajectory_delay_time_offset(); i_param.swing_trajectory_final_distance_weight = gg->get_swing_trajectory_final_distance_weight(); + i_param.swing_trajectory_time_offset_xy2z = gg->get_swing_trajectory_time_offset_xy2z(); i_param.gravitational_acceleration = gg->get_gravitational_acceleration(); i_param.toe_angle = gg->get_toe_angle(); i_param.heel_angle = gg->get_heel_angle(); @@ -1468,11 +2994,14 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener i_param.heel_pos_offset_x = gg->get_heel_pos_offset_x(); i_param.toe_zmp_offset_x = gg->get_toe_zmp_offset_x(); i_param.heel_zmp_offset_x = gg->get_heel_zmp_offset_x(); + i_param.toe_check_thre = gg->get_toe_check_thre(); + i_param.heel_check_thre = gg->get_heel_check_thre(); std::vector ratio(gg->get_NUM_TH_PHASES(),0.0); gg->get_toe_heel_phase_ratio(ratio); for (int i = 0; i < gg->get_NUM_TH_PHASES(); i++) i_param.toe_heel_phase_ratio[i] = ratio[i]; i_param.use_toe_joint = gg->get_use_toe_joint(); i_param.use_toe_heel_transition = gg->get_use_toe_heel_transition(); + i_param.use_toe_heel_auto_set = gg->get_use_toe_heel_auto_set(); std::map tmp_zmp_weight_map = gg->get_zmp_weight_map(); i_param.zmp_weight_map[0] = tmp_zmp_weight_map[RLEG]; i_param.zmp_weight_map[1] = tmp_zmp_weight_map[LLEG]; @@ -1480,6 +3009,68 @@ bool AutoBalancer::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGener i_param.zmp_weight_map[3] = tmp_zmp_weight_map[LARM]; i_param.optional_go_pos_finalize_footstep_num = gg->get_optional_go_pos_finalize_footstep_num(); i_param.overwritable_footstep_index_offset = gg->get_overwritable_footstep_index_offset(); + for (size_t i=0; i<4; i++) { + i_param.leg_margin[i] = gg->get_leg_margin(i); + } + for (size_t i=0; i<4; i++) { + i_param.safe_leg_margin[i] = gg->get_safe_leg_margin(i); + } + for (size_t i=0; i<5; i++) { + i_param.stride_limitation_for_circle_type[i] = gg->get_stride_limitation_for_circle_type(i); + } + for (size_t i=0; i<5; i++) { + i_param.overwritable_stride_limitation[i] = gg->get_overwritable_stride_limitation(i); + } + i_param.use_stride_limitation = gg->get_use_stride_limitation(); + i_param.footstep_modification_gain = gg->get_footstep_modification_gain(); + i_param.modify_footsteps = gg->get_modify_footsteps(); + i_param.min_time_mgn = gg->get_min_time_mgn(); + i_param.min_time = gg->get_min_time(); + for (size_t i=0; i<2; i++) { + i_param.cp_check_margin[i] = gg->get_cp_check_margin(i); + } + i_param.margin_time_ratio = gg->get_margin_time_ratio(); + if (gg->get_stride_limitation_type() == SQUARE) { + i_param.stride_limitation_type = OpenHRP::AutoBalancerService::SQUARE; + } else if (gg->get_stride_limitation_type() == CIRCLE) { + i_param.stride_limitation_type = OpenHRP::AutoBalancerService::CIRCLE; + } + i_param.act_vel_ratio = gg->get_act_vel_ratio(); + i_param.use_disturbance_compensation = gg->get_use_disturbance_compensation(); + i_param.dc_gain = gg->get_dc_gain(); + i_param.dcm_offset = gg->get_dcm_offset(); + for (size_t i = 0; i < 3; i++) { + i_param.emergency_step_time[i] = gg->get_emergency_step_time(i); + } + i_param.use_act_states = gg->use_act_states; + i_param.is_interpolate_zmp_in_double = gg->is_interpolate_zmp_in_double; + i_param.is_stable_go_stop_mode = gg->is_stable_go_stop_mode; + i_param.use_flywheel_balance = gg->use_flywheel_balance; + i_param.zmp_delay_time_const = gg->zmp_delay_time_const; + i_param.overwritable_max_time = gg->overwritable_max_time; + i_param.fg_zmp_cutoff_freq = gg->fg_zmp_cutoff_freq; + i_param.fg_cp_cutoff_freq = gg->cp_filter->getCutOffFreq(); + { + hrp::Vector3 thre, delta; + gg->get_sum_d_footstep_thre(thre); + gg->get_footstep_check_delta(delta); + for (size_t i = 0; i < 3; i++) { + i_param.sum_d_footstep_thre[i] = thre(i); + i_param.footstep_check_delta[i] = delta(i); + } + } + i_param.debug_set_landing_height = gg->debug_set_landing_height; + i_param.debug_landing_height = gg->debug_landing_height; + for (size_t i = 0; i < 2; i++) { + i_param.debug_landing_height_xrange[i] = gg->debug_landing_height_xrange[i]; + } + for (size_t i = 0; i < 2; i++) { + i_param.front_edge_offset_of_steppable_region[i] = gg->front_edge_offset_of_steppable_region[i]; + } + i_param.is_slow_stair_mode = gg->is_slow_stair_mode; + i_param.stair_step_time = gg->stair_step_time; + i_param.num_preview_step = gg->num_preview_step; + i_param.rectangle_time_smooth_offset = gg->get_rectangle_time_smooth_offset(); return true; }; @@ -1488,15 +3079,16 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto Guard guard(m_mutex); std::cerr << "[" << m_profile.instance_name << "] setAutoBalancerParam" << std::endl; double *default_zmp_offsets_array = new double[ikp.size()*3]; - move_base_gain = i_param.move_base_gain; for (size_t i = 0; i < ikp.size(); i++) for (size_t j = 0; j < 3; j++) default_zmp_offsets_array[i*3+j] = i_param.default_zmp_offsets[i][j]; zmp_transition_time = i_param.zmp_transition_time; adjust_footstep_transition_time = i_param.adjust_footstep_transition_time; - if (zmp_offset_interpolator->isEmpty()) { + if (control_mode == MODE_IDLE) { + zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true); + } else if (zmp_offset_interpolator->isEmpty()) { zmp_offset_interpolator->clear(); - zmp_offset_interpolator->go(default_zmp_offsets_array, zmp_transition_time, true); + zmp_offset_interpolator->setGoal(default_zmp_offsets_array, zmp_transition_time, true); } else { std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets cannot be set because interpolating." << std::endl; } @@ -1508,6 +3100,15 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto case OpenHRP::AutoBalancerService::MODE_REF_FORCE: use_force = MODE_REF_FORCE; break; + case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT: + use_force = MODE_REF_FORCE_WITH_FOOT; + break; + case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT: + use_force = MODE_REF_FORCE_RFU_EXT_MOMENT; + break; + case OpenHRP::AutoBalancerService::MODE_ADDITIONAL_FORCE: + use_force = MODE_ADDITIONAL_FORCE; + break; default: break; } @@ -1540,15 +3141,13 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto double tmp_ratio = 0.0; leg_names_interpolator->set(&tmp_ratio); tmp_ratio = 1.0; - leg_names_interpolator->go(&tmp_ratio, 5.0, true); + leg_names_interpolator->setGoal(&tmp_ratio, 5.0, true); control_mode = MODE_SYNC_TO_ABC; } } } else { std::cerr << "[" << m_profile.instance_name << "] leg_names cannot be set because interpolating." << std::endl; } - pos_ik_thre = i_param.pos_ik_thre; - rot_ik_thre = i_param.rot_ik_thre; if (!gg_is_walking) { is_hand_fix_mode = i_param.is_hand_fix_mode; std::cerr << "[" << m_profile.instance_name << "] is_hand_fix_mode = " << is_hand_fix_mode << std::endl; @@ -1575,20 +3174,33 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto } else if (i_param.default_gait_type == OpenHRP::AutoBalancerService::GALLOP) { gait_type = GALLOP; } + // Ref force balancing + std::cerr << "[" << m_profile.instance_name << "] Ref force balancing" << std::endl; + if ( use_force == MODE_REF_FORCE_WITH_FOOT && control_mode != MODE_IDLE ) { + std::cerr << "[" << m_profile.instance_name << "] additional_force_applied_point_offset and additional_force_applied_link_name cannot be updated during MODE_REF_FORCE_WITH_FOOT and non-MODE_IDLE"<< std::endl; + } else if ( !m_robot->link(std::string(i_param.additional_force_applied_link_name)) ) { + std::cerr << "[" << m_profile.instance_name << "] Invalid link name for additional_force_applied_link_name = " << i_param.additional_force_applied_link_name << std::endl; + } else { + additional_force_applied_link = m_robot->link(std::string(i_param.additional_force_applied_link_name)); + for (size_t i = 0; i < 3; i++) { + additional_force_applied_point_offset(i) = i_param.additional_force_applied_point_offset[i]; + } + std::cerr << "[" << m_profile.instance_name << "] Link name for additional_force_applied_link_name = " << additional_force_applied_link->name << ", additional_force_applied_point_offset = " << additional_force_applied_point_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + } + for (std::map::iterator it = ikp.begin(); it != ikp.end(); it++) { std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->first << "]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->second.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] localR = " << it->second.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; } - std::cerr << "[" << m_profile.instance_name << "] move_base_gain = " << move_base_gain << std::endl; std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets = "; for (size_t i = 0; i < ikp.size() * 3; i++) { std::cerr << default_zmp_offsets_array[i] << " "; } std::cerr << std::endl; delete[] default_zmp_offsets_array; - std::cerr << "[" << m_profile.instance_name << "] use_force_mode = " << use_force << std::endl; + std::cerr << "[" << m_profile.instance_name << "] use_force_mode = " << getUseForceModeString() << std::endl; std::cerr << "[" << m_profile.instance_name << "] graspless_manip_mode = " << graspless_manip_mode << std::endl; std::cerr << "[" << m_profile.instance_name << "] graspless_manip_arm = " << graspless_manip_arm << std::endl; std::cerr << "[" << m_profile.instance_name << "] graspless_manip_p_gain = " << graspless_manip_p_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; @@ -1596,87 +3208,53 @@ bool AutoBalancer::setAutoBalancerParam(const OpenHRP::AutoBalancerService::Auto std::cerr << "[" << m_profile.instance_name << "] graspless_manip_reference_trans_rot = " << graspless_manip_reference_trans_coords.rot.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; std::cerr << "[" << m_profile.instance_name << "] transition_time = " << transition_time << "[s], zmp_transition_time = " << zmp_transition_time << "[s], adjust_footstep_transition_time = " << adjust_footstep_transition_time << "[s]" << std::endl; for (std::vector::iterator it = leg_names.begin(); it != leg_names.end(); it++) std::cerr << "[" << m_profile.instance_name << "] leg_names [" << *it << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] default_gait_type = " << gait_type << std::endl; + // FIK + fik->move_base_gain = i_param.move_base_gain; + fik->pos_ik_thre = i_param.pos_ik_thre; + fik->rot_ik_thre = i_param.rot_ik_thre; + fik->printParam(); // IK limb parameters - std::cerr << "[" << m_profile.instance_name << "] IK limb parameters" << std::endl; - bool is_ik_limb_parameter_valid_length = true; - if (i_param.ik_limb_parameters.length() != ee_vec.size()) { - is_ik_limb_parameter_valid_length = false; - std::cerr << "[" << m_profile.instance_name << "] ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl; + fik->setIKParam(ee_vec, i_param.ik_limb_parameters); + // Limb stretch avoidance + if (i_param.use_limb_stretch_avoidance) std::cerr << "[" << m_profile.instance_name << "] use_limb_stretch_avoidance is invalid in current version" << std::endl; + // fik->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance; + fik->limb_stretch_avoidance_time_const = i_param.limb_stretch_avoidance_time_const; + for (size_t i = 0; i < 2; i++) { + fik->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i]; + } + for (size_t i = 0; i < fik->ikp.size(); i++) { + fik->ikp[ee_vec[i]].limb_length_margin = i_param.limb_length_margin[i]; + } + is_emergency_step_mode = i_param.is_emergency_step_mode; + is_emergency_touch_wall_mode = i_param.is_emergency_touch_wall_mode; + use_collision_avoidance = i_param.use_collision_avoidance; + if (control_mode == MODE_IDLE) { + ik_mode = i_param.ik_mode; + std::cerr << "[" << m_profile.instance_name << "] ik_mode = " << ik_mode << std::endl; } else { - for (size_t i = 0; i < ee_vec.size(); i++) { - if (ikp[ee_vec[i]].manip->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length()) - is_ik_limb_parameter_valid_length = false; - } - if (is_ik_limb_parameter_valid_length) { - for (size_t i = 0; i < ee_vec.size(); i++) { - ABCIKparam& param = ikp[ee_vec[i]]; - const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i]; - std::vector ov; - ov.resize(param.manip->numJoints()); - for (size_t j = 0; j < param.manip->numJoints(); j++) { - ov[j] = ilp.ik_optional_weight_vector[j]; - } - param.manip->setOptionalWeightVector(ov); - param.manip->setSRGain(ilp.sr_gain); - param.avoid_gain = ilp.avoid_gain; - param.reference_gain = ilp.reference_gain; - param.manip->setManipulabilityLimit(ilp.manipulability_limit); - } - } else { - std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vector invalid length! Cannot be set. (input = ["; - for (size_t i = 0; i < ee_vec.size(); i++) { - std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", "; - } - std::cerr << "], desired = ["; - for (size_t i = 0; i < ee_vec.size(); i++) { - std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", "; - } - std::cerr << "])" << std::endl; - } + std::cerr << "[" << m_profile.instance_name << "] ik_mode cannot be set in (control_mode != MODE_IDLE). Current ik_mode is " << ik_mode << std::endl; } - if (is_ik_limb_parameter_valid_length) { - std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vectors = "; - for (size_t i = 0; i < ee_vec.size(); i++) { - ABCIKparam& param = ikp[ee_vec[i]]; - std::vector ov; - ov.resize(param.manip->numJoints()); - param.manip->getOptionalWeightVector(ov); - std::cerr << "["; - for (size_t j = 0; j < param.manip->numJoints(); j++) { - std::cerr << ov[j] << " "; - } - std::cerr << "]"; - } - std::cerr << std::endl; - std::cerr << "[" << m_profile.instance_name << "] sr_gains = ["; - for (size_t i = 0; i < ee_vec.size(); i++) { - std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", "; - } - std::cerr << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] avoid_gains = ["; - for (size_t i = 0; i < ee_vec.size(); i++) { - std::cerr << ikp[ee_vec[i]].avoid_gain << ", "; - } - std::cerr << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] reference_gains = ["; - for (size_t i = 0; i < ee_vec.size(); i++) { - std::cerr << ikp[ee_vec[i]].reference_gain << ", "; - } - std::cerr << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] manipulability_limits = ["; - for (size_t i = 0; i < ee_vec.size(); i++) { - std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", "; - } - std::cerr << "]" << std::endl; + if (cog_constraint_interpolator->isEmpty()) { + double tmp_time = 1.0; + cog_constraint_interpolator->clear(); + cog_constraint_interpolator->set(&cog_z_constraint); + cog_constraint_interpolator->setGoal(&i_param.cog_z_constraint, tmp_time, true); + } else { + std::cerr << "[" << m_profile.instance_name << "] cog_z_constraint cannot be set because interpolating." << std::endl; } + touch_wall_retrieve_time = i_param.touch_wall_retrieve_time; + is_natural_walk = i_param.is_natural_walk; + is_stop_early_foot = i_param.is_stop_early_foot; + gg->set_is_stop_early_foot(is_stop_early_foot); + arm_swing_deg = i_param.arm_swing_deg; + debug_read_steppable_region = i_param.debug_read_steppable_region; + ik_max_loop = i_param.ik_max_loop; return true; }; bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam& i_param) { - i_param.move_base_gain = move_base_gain; i_param.default_zmp_offsets.length(ikp.size()); for (size_t i = 0; i < ikp.size(); i++) { i_param.default_zmp_offsets[i].length(3); @@ -1694,6 +3272,9 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc switch(use_force) { case MODE_NO_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_NO_FORCE; break; case MODE_REF_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE; break; + case MODE_REF_FORCE_WITH_FOOT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT; break; + case MODE_REF_FORCE_RFU_EXT_MOMENT: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT; break; + case MODE_ADDITIONAL_FORCE: i_param.use_force_mode = OpenHRP::AutoBalancerService::MODE_ADDITIONAL_FORCE; break; default: break; } i_param.graspless_manip_mode = graspless_manip_mode; @@ -1712,8 +3293,6 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc i_param.adjust_footstep_transition_time = adjust_footstep_transition_time; i_param.leg_names.length(leg_names.size()); for (size_t i = 0; i < leg_names.size(); i++) i_param.leg_names[i] = leg_names.at(i).c_str(); - i_param.pos_ik_thre = pos_ik_thre; - i_param.rot_ik_thre = rot_ik_thre; i_param.is_hand_fix_mode = is_hand_fix_mode; i_param.end_effector_list.length(ikp.size()); { @@ -1733,25 +3312,635 @@ bool AutoBalancer::getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalanc case GALLOP: i_param.default_gait_type = OpenHRP::AutoBalancerService::GALLOP; break; default: break; } - i_param.ik_limb_parameters.length(ee_vec.size()); - for (size_t i = 0; i < ee_vec.size(); i++) { - ABCIKparam& param = ikp[ee_vec[i]]; - OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i]; - ilp.ik_optional_weight_vector.length(param.manip->numJoints()); - std::vector ov; - ov.resize(param.manip->numJoints()); - param.manip->getOptionalWeightVector(ov); - for (size_t j = 0; j < param.manip->numJoints(); j++) { - ilp.ik_optional_weight_vector[j] = ov[j]; - } - ilp.sr_gain = param.manip->getSRGain(); - ilp.avoid_gain = param.avoid_gain; - ilp.reference_gain = param.reference_gain; - ilp.manipulability_limit = param.manip->getManipulabilityLimit(); + // FIK + i_param.move_base_gain = fik->move_base_gain; + i_param.pos_ik_thre = fik->pos_ik_thre; + i_param.rot_ik_thre = fik->rot_ik_thre; + // IK limb parameters + fik->getIKParam(ee_vec, i_param.ik_limb_parameters); + // Limb stretch avoidance + i_param.use_limb_stretch_avoidance = fik->use_limb_stretch_avoidance; + i_param.limb_stretch_avoidance_time_const = fik->limb_stretch_avoidance_time_const; + i_param.limb_length_margin.length(fik->ikp.size()); + for (size_t i = 0; i < 2; i++) { + i_param.limb_stretch_avoidance_vlimit[i] = fik->limb_stretch_avoidance_vlimit[i]; + } + for (size_t i = 0; i < ikp.size(); i++) { + i_param.limb_length_margin[i] = fik->ikp[ee_vec[i]].limb_length_margin; } + i_param.additional_force_applied_link_name = additional_force_applied_link->name.c_str(); + for (size_t i = 0; i < 3; i++) { + i_param.additional_force_applied_point_offset[i] = additional_force_applied_point_offset(i); + } + i_param.is_emergency_step_mode = is_emergency_step_mode; + i_param.is_emergency_touch_wall_mode = is_emergency_touch_wall_mode; + i_param.ik_mode = ik_mode; + i_param.use_collision_avoidance = use_collision_avoidance; + i_param.cog_z_constraint = cog_z_constraint; + i_param.touch_wall_retrieve_time = touch_wall_retrieve_time; + i_param.is_natural_walk = is_natural_walk; + i_param.is_stop_early_foot = is_stop_early_foot; + i_param.arm_swing_deg = arm_swing_deg; + i_param.debug_read_steppable_region = debug_read_steppable_region; + i_param.ik_max_loop = ik_max_loop; return true; }; +void AutoBalancer::setStabilizerParam(const OpenHRP::AutoBalancerService::StabilizerParam& i_param) +{ + Guard guard(m_mutex); + std::cerr << "[" << m_profile.instance_name << "] setStabilizerParam" << std::endl; + for (size_t i = 0; i < 2; i++) { + st->k_tpcc_p[i] = i_param.k_tpcc_p[i]; + st->k_tpcc_x[i] = i_param.k_tpcc_x[i]; + st->k_brot_p[i] = i_param.k_brot_p[i]; + st->k_brot_tc[i] = i_param.k_brot_tc[i]; + } + std::cerr << "[" << m_profile.instance_name << "] TPCC" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] k_tpcc_p = [" << st->k_tpcc_p[0] << ", " << st->k_tpcc_p[1] << "], k_tpcc_x = [" << st->k_tpcc_x[0] << ", " << st->k_tpcc_x[1] << "], k_brot_p = [" << st->k_brot_p[0] << ", " << st->k_brot_p[1] << "], k_brot_tc = [" << st->k_brot_tc[0] << ", " << st->k_brot_tc[1] << "]" << std::endl; + // for (size_t i = 0; i < 2; i++) { + // k_run_b[i] = i_param.k_run_b[i]; + // d_run_b[i] = i_param.d_run_b[i]; + // m_tau_x[i].setup(i_param.tdfke[0], i_param.tdftc[0], dt); + // m_tau_y[i].setup(i_param.tdfke[0], i_param.tdftc[0], dt); + // m_f_z.setup(i_param.tdfke[1], i_param.tdftc[1], dt); + // } + // m_torque_k[0] = i_param.k_run_x; + // m_torque_k[1] = i_param.k_run_y; + // m_torque_d[0] = i_param.d_run_x; + // m_torque_d[1] = i_param.d_run_y; + // std::cerr << "[" << m_profile.instance_name << "] RUNST" << std::endl; + // std::cerr << "[" << m_profile.instance_name << "] m_torque_k = [" << m_torque_k[0] << ", " << m_torque_k[1] << "]" << std::endl; + // std::cerr << "[" << m_profile.instance_name << "] m_torque_d = [" << m_torque_d[0] << ", " << m_torque_d[1] << "]" << std::endl; + // std::cerr << "[" << m_profile.instance_name << "] k_run_b = [" << k_run_b[0] << ", " << k_run_b[1] << "]" << std::endl; + // std::cerr << "[" << m_profile.instance_name << "] d_run_b = [" << d_run_b[0] << ", " << d_run_b[1] << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] EEFM" << std::endl; + for (size_t i = 0; i < 2; i++) { + st->eefm_k1[i] = i_param.eefm_k1[i]; + st->eefm_k2[i] = i_param.eefm_k2[i]; + st->eefm_k3[i] = i_param.eefm_k3[i]; + st->eefm_zmp_delay_time_const[i] = i_param.eefm_zmp_delay_time_const[i]; + st->ref_zmp_aux(i) = i_param.eefm_ref_zmp_aux[i]; + st->eefm_body_attitude_control_gain[i] = i_param.eefm_body_attitude_control_gain[i]; + st->eefm_body_attitude_control_time_const[i] = i_param.eefm_body_attitude_control_time_const[i]; + st->ref_cp(i) = i_param.ref_capture_point[i]; + st->act_cp(i) = i_param.act_capture_point[i]; + st->cp_offset(i) = i_param.cp_offset[i]; + } + bool is_damping_parameter_ok = true; + if ( i_param.eefm_pos_damping_gain.length () == st->stikp.size() && + i_param.eefm_pos_time_const_support.length () == st->stikp.size() && + i_param.eefm_pos_compensation_limit.length () == st->stikp.size() && + i_param.eefm_swing_pos_spring_gain.length () == st->stikp.size() && + i_param.eefm_swing_pos_time_const.length () == st->stikp.size() && + i_param.eefm_rot_damping_gain.length () == st->stikp.size() && + i_param.eefm_rot_time_const.length () == st->stikp.size() && + i_param.eefm_rot_compensation_limit.length () == st->stikp.size() && + i_param.eefm_swing_rot_spring_gain.length () == st->stikp.size() && + i_param.eefm_swing_rot_time_const.length () == st->stikp.size() && + i_param.eefm_ee_moment_limit.length () == st->stikp.size() && + i_param.eefm_ee_forcemoment_distribution_weight.length () == st->stikp.size()) { + is_damping_parameter_ok = true; + for (size_t j = 0; j < st->stikp.size(); j++) { + for (size_t i = 0; i < 3; i++) { + st->stikp[j].eefm_pos_damping_gain(i) = i_param.eefm_pos_damping_gain[j][i]; + st->stikp[j].eefm_pos_time_const_support(i) = i_param.eefm_pos_time_const_support[j][i]; + st->stikp[j].eefm_swing_pos_spring_gain(i) = i_param.eefm_swing_pos_spring_gain[j][i]; + st->stikp[j].eefm_swing_pos_time_const(i) = i_param.eefm_swing_pos_time_const[j][i]; + st->stikp[j].eefm_rot_damping_gain(i) = i_param.eefm_rot_damping_gain[j][i]; + st->stikp[j].eefm_rot_time_const(i) = i_param.eefm_rot_time_const[j][i]; + st->stikp[j].eefm_swing_rot_spring_gain(i) = i_param.eefm_swing_rot_spring_gain[j][i]; + st->stikp[j].eefm_swing_rot_time_const(i) = i_param.eefm_swing_rot_time_const[j][i]; + st->stikp[j].eefm_ee_moment_limit(i) = i_param.eefm_ee_moment_limit[j][i]; + st->stikp[j].eefm_ee_forcemoment_distribution_weight(i) = i_param.eefm_ee_forcemoment_distribution_weight[j][i]; + st->stikp[j].eefm_ee_forcemoment_distribution_weight(i+3) = i_param.eefm_ee_forcemoment_distribution_weight[j][i+3]; + } + st->stikp[j].eefm_pos_compensation_limit = i_param.eefm_pos_compensation_limit[j]; + st->stikp[j].eefm_rot_compensation_limit = i_param.eefm_rot_compensation_limit[j]; + } + } else { + is_damping_parameter_ok = false; + } + for (size_t i = 0; i < 3; i++) { + st->eefm_swing_pos_damping_gain(i) = i_param.eefm_swing_pos_damping_gain[i]; + st->eefm_swing_rot_damping_gain(i) = i_param.eefm_swing_rot_damping_gain[i]; + } + st->eefm_pos_time_const_swing = i_param.eefm_pos_time_const_swing; + st->eefm_pos_transition_time = i_param.eefm_pos_transition_time; + st->eefm_pos_margin_time = i_param.eefm_pos_margin_time; + st->szd->set_leg_inside_margin(i_param.eefm_leg_inside_margin); + st->szd->set_leg_outside_margin(i_param.eefm_leg_outside_margin); + st->szd->set_leg_front_margin(i_param.eefm_leg_front_margin); + st->szd->set_leg_rear_margin(i_param.eefm_leg_rear_margin); + st->szd->set_vertices_from_margin_params(); + + if (i_param.eefm_support_polygon_vertices_sequence.length() != st->stikp.size()) { + std::cerr << "[" << m_profile.instance_name << "] eefm_support_polygon_vertices_sequence cannot be set. Length " << i_param.eefm_support_polygon_vertices_sequence.length() << " != " << st->stikp.size() << std::endl; + } else { + std::cerr << "[" << m_profile.instance_name << "] eefm_support_polygon_vertices_sequence set" << std::endl; + std::vector > support_polygon_vec; + for (size_t ee_idx = 0; ee_idx < i_param.eefm_support_polygon_vertices_sequence.length(); ee_idx++) { + std::vector tvec; + for (size_t v_idx = 0; v_idx < i_param.eefm_support_polygon_vertices_sequence[ee_idx].vertices.length(); v_idx++) { + tvec.push_back(Eigen::Vector2d(i_param.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[0], + i_param.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[1])); + } + support_polygon_vec.push_back(tvec); + } + st->szd->set_vertices(support_polygon_vec); + st->szd->print_vertices(std::string(m_profile.instance_name)); + } + st->eefm_use_force_difference_control = i_param.eefm_use_force_difference_control; + st->eefm_use_swing_damping = i_param.eefm_use_swing_damping; + for (size_t i = 0; i < 3; ++i) { + st->eefm_swing_damping_force_thre[i] = i_param.eefm_swing_damping_force_thre[i]; + st->eefm_swing_damping_moment_thre[i] = i_param.eefm_swing_damping_moment_thre[i]; + } + st->act_cogvel_filter->setCutOffFreq(i_param.eefm_cogvel_cutoff_freq); + st->szd->set_wrench_alpha_blending(i_param.eefm_wrench_alpha_blending); + st->szd->set_alpha_cutoff_freq(i_param.eefm_alpha_cutoff_freq); + st->eefm_gravitational_acceleration = i_param.eefm_gravitational_acceleration; + for (size_t i = 0; i < st->stikp.size(); i++) { + st->stikp[i].target_ee_diff_p_filter->setCutOffFreq(i_param.eefm_ee_error_cutoff_freq); + st->stikp[i].limb_length_margin = i_param.limb_length_margin[i]; + } + st->setBoolSequenceParam(st->is_ik_enable, i_param.is_ik_enable, std::string("is_ik_enable")); + st->setBoolSequenceParamWithCheckContact(st->is_feedback_control_enable, i_param.is_feedback_control_enable, std::string("is_feedback_control_enable")); + st->setBoolSequenceParam(st->is_zmp_calc_enable, i_param.is_zmp_calc_enable, std::string("is_zmp_calc_enable")); + st->emergency_check_mode = i_param.emergency_check_mode; + + st->transition_time = i_param.transition_time; + st->cop_check_margin = i_param.cop_check_margin; + for (size_t i = 0; i < st->cp_check_margin.size(); i++) { + st->cp_check_margin[i] = i_param.cp_check_margin[i]; + } + st->szd->set_vertices_from_margin_params(st->cp_check_margin); + for (size_t i = 0; i < st->tilt_margin.size(); i++) { + st->tilt_margin[i] = i_param.tilt_margin[i]; + } + st->contact_decision_threshold = i_param.contact_decision_threshold; + st->is_estop_while_walking = i_param.is_estop_while_walking; + st->use_limb_stretch_avoidance = i_param.use_limb_stretch_avoidance; + st->use_zmp_truncation = i_param.use_zmp_truncation; + st->use_force_sensor = i_param.use_force_sensor; + gg->set_use_force_sensor(st->use_force_sensor); + st->use_footguided_stabilizer = i_param.use_footguided_stabilizer; + gg->footguided_balance_time_const = st->footguided_balance_time_const = i_param.footguided_balance_time_const; + for (size_t i = 0; i < 2; i++) { + st->limb_stretch_avoidance_vlimit[i] = i_param.limb_stretch_avoidance_vlimit[i]; + st->root_rot_compensation_limit[i] = i_param.root_rot_compensation_limit[i]; + } + st->detection_count_to_air = static_cast(i_param.detection_time_to_air / m_dt); + if (st->control_mode == Stabilizer::MODE_IDLE) { + for (size_t i = 0; i < i_param.end_effector_list.length(); i++) { + std::vector::iterator it = std::find_if(st->stikp.begin(), st->stikp.end(), (&boost::lambda::_1->* &std::vector::value_type::ee_name == std::string(i_param.end_effector_list[i].leg))); + memcpy(it->localp.data(), i_param.end_effector_list[i].pos, sizeof(double)*3); + it->localR = (Eigen::Quaternion(i_param.end_effector_list[i].rot[0], i_param.end_effector_list[i].rot[1], i_param.end_effector_list[i].rot[2], i_param.end_effector_list[i].rot[3])).normalized().toRotationMatrix(); + } + } else { + std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl; + } + for (std::vector::const_iterator it = st->stikp.begin(); it != st->stikp.end(); it++) { + std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->ee_name << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->localp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localR = " << it->localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "", " [", "]")) << std::endl; + } + if (i_param.foot_origin_offset.length () != 2) { + std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset cannot be set. Length " << i_param.foot_origin_offset.length() << " != " << 2 << std::endl; + } else if (st->control_mode != Stabilizer::MODE_IDLE) { + std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset cannot be set. Current control_mode is " << st->control_mode << std::endl; + } else { + for (size_t i = 0; i < i_param.foot_origin_offset.length(); i++) { + st->foot_origin_offset[i](0) = i_param.foot_origin_offset[i][0]; + st->foot_origin_offset[i](1) = i_param.foot_origin_offset[i][1]; + st->foot_origin_offset[i](2) = i_param.foot_origin_offset[i][2]; + } + } + std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset is "; + for (size_t i = 0; i < 2; i++) { + std::cerr << st->foot_origin_offset[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")); + } + std::cerr << "[m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_k1 = [" << st->eefm_k1[0] << ", " << st->eefm_k1[1] << "], eefm_k2 = [" << st->eefm_k2[0] << ", " << st->eefm_k2[1] << "], eefm_k3 = [" << st->eefm_k3[0] << ", " << st->eefm_k3[1] << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_zmp_delay_time_const = [" << st->eefm_zmp_delay_time_const[0] << ", " << st->eefm_zmp_delay_time_const[1] << "][s], eefm_ref_zmp_aux = [" << st->ref_zmp_aux(0) << ", " << st->ref_zmp_aux(1) << "][m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_body_attitude_control_gain = [" << st->eefm_body_attitude_control_gain[0] << ", " << st->eefm_body_attitude_control_gain[1] << "], eefm_body_attitude_control_time_const = [" << st->eefm_body_attitude_control_time_const[0] << ", " << st->eefm_body_attitude_control_time_const[1] << "][s]" << std::endl; + if (is_damping_parameter_ok) { + for (size_t j = 0; j < st->stikp.size(); j++) { + std::cerr << "[" << m_profile.instance_name << "] [" << st->stikp[j].ee_name << "] eefm_rot_damping_gain = " + << st->stikp[j].eefm_rot_damping_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", eefm_rot_time_const = " + << st->stikp[j].eefm_rot_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << st->stikp[j].ee_name << "] eefm_pos_damping_gain = " + << st->stikp[j].eefm_pos_damping_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", eefm_pos_time_const_support = " + << st->stikp[j].eefm_pos_time_const_support.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << st->stikp[j].ee_name << "] " + << "eefm_pos_compensation_limit = " << st->stikp[j].eefm_pos_compensation_limit << "[m], " + << "eefm_rot_compensation_limit = " << st->stikp[j].eefm_rot_compensation_limit << "[rad], " + << "eefm_ee_moment_limit = " << st->stikp[j].eefm_ee_moment_limit.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << st->stikp[j].ee_name << "] " + << "eefm_swing_pos_spring_gain = " << st->stikp[j].eefm_swing_pos_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " + << "eefm_swing_pos_time_const = " << st->stikp[j].eefm_swing_pos_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " + << "eefm_swing_rot_spring_gain = " << st->stikp[j].eefm_swing_rot_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " + << "eefm_swing_pos_time_const = " << st->stikp[j].eefm_swing_pos_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " + << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << st->stikp[j].ee_name << "] " + << "eefm_ee_forcemoment_distribution_weight = " << st->stikp[j].eefm_ee_forcemoment_distribution_weight.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "" << std::endl; + } + } else { + std::cerr << "[" << m_profile.instance_name << "] eefm damping parameters cannot be set because of invalid param." << std::endl; + } + std::cerr << "[" << m_profile.instance_name << "] eefm_pos_transition_time = " << st->eefm_pos_transition_time << "[s], eefm_pos_margin_time = " << st->eefm_pos_margin_time << "[s] eefm_pos_time_const_swing = " << st->eefm_pos_time_const_swing << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] cogvel_cutoff_freq = " << st->act_cogvel_filter->getCutOffFreq() << "[Hz]" << std::endl; + st->szd->print_params(std::string(m_profile.instance_name)); + std::cerr << "[" << m_profile.instance_name << "] eefm_gravitational_acceleration = " << st->eefm_gravitational_acceleration << "[m/s^2], eefm_use_force_difference_control = " << (st->eefm_use_force_difference_control? "true":"false") << ", eefm_use_swing_damping = " << (st->eefm_use_swing_damping? "true":"false") << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_ee_error_cutoff_freq = " << st->stikp[0].target_ee_diff_p_filter->getCutOffFreq() << "[Hz]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] COMMON" << std::endl; + if (st->control_mode == Stabilizer::MODE_IDLE) { + st->st_algorithm = i_param.st_algorithm; + std::cerr << "[" << m_profile.instance_name << "] st_algorithm changed to [" << st->getStabilizerAlgorithmString(st->st_algorithm) << "]" << std::endl; + } else { + std::cerr << "[" << m_profile.instance_name << "] st_algorithm cannot be changed to [" << st->getStabilizerAlgorithmString(st->st_algorithm) << "] during MODE_AIR or MODE_ST." << std::endl; + } + std::cerr << "[" << m_profile.instance_name << "] emergency_check_mode changed to [" << (st->emergency_check_mode == OpenHRP::AutoBalancerService::NO_CHECK?"NO_CHECK": (st->emergency_check_mode == OpenHRP::AutoBalancerService::COP?"COP":"CP") ) << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] transition_time = " << st->transition_time << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] cop_check_margin = " << st->cop_check_margin << "[m], " + << "cp_check_margin = [" << st->cp_check_margin[0] << ", " << st->cp_check_margin[1] << ", " << st->cp_check_margin[2] << ", " << st->cp_check_margin[3] << "] [m], " + << "tilt_margin = [" << st->tilt_margin[0] << ", " << st->tilt_margin[1] << "] [rad]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] contact_decision_threshold = " << st->contact_decision_threshold << "[N], detection_time_to_air = " << st->detection_count_to_air * m_dt << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] root_rot_compensation_limit = [" << st->root_rot_compensation_limit[0] << " " << st->root_rot_compensation_limit[1] << "][rad]" << std::endl; + // IK limb parameters + std::cerr << "[" << m_profile.instance_name << "] IK limb parameters" << std::endl; + bool is_ik_limb_parameter_valid_length = true; + if (i_param.ik_limb_parameters.length() != st->jpe_v.size()) { + is_ik_limb_parameter_valid_length = false; + std::cerr << "[" << m_profile.instance_name << "] ik_limb_parameters invalid length! Cannot be set. (input = " << i_param.ik_limb_parameters.length() << ", desired = " << st->jpe_v.size() << ")" << std::endl; + } else { + for (size_t i = 0; i < st->jpe_v.size(); i++) { + if (st->jpe_v[i]->numJoints() != i_param.ik_limb_parameters[i].ik_optional_weight_vector.length()) + is_ik_limb_parameter_valid_length = false; + } + if (is_ik_limb_parameter_valid_length) { + for (size_t i = 0; i < st->jpe_v.size(); i++) { + const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i]; + std::vector ov; + ov.resize(st->jpe_v[i]->numJoints()); + for (size_t j = 0; j < st->jpe_v[i]->numJoints(); j++) { + ov[j] = ilp.ik_optional_weight_vector[j]; + } + st->jpe_v[i]->setOptionalWeightVector(ov); + st->jpe_v[i]->setSRGain(ilp.sr_gain); + st->stikp[i].avoid_gain = ilp.avoid_gain; + st->stikp[i].reference_gain = ilp.reference_gain; + st->jpe_v[i]->setManipulabilityLimit(ilp.manipulability_limit); + st->stikp[i].ik_loop_count = ilp.ik_loop_count; // unsigned short -> size_t, value not change + } + } else { + std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vector invalid length! Cannot be set. (input = ["; + for (size_t i = 0; i < st->jpe_v.size(); i++) { + std::cerr << i_param.ik_limb_parameters[i].ik_optional_weight_vector.length() << ", "; + } + std::cerr << "], desired = ["; + for (size_t i = 0; i < st->jpe_v.size(); i++) { + std::cerr << st->jpe_v[i]->numJoints() << ", "; + } + std::cerr << "])" << std::endl; + } + } + if (is_ik_limb_parameter_valid_length) { + std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vectors = "; + for (size_t i = 0; i < st->jpe_v.size(); i++) { + std::vector ov; + ov.resize(st->jpe_v[i]->numJoints()); + st->jpe_v[i]->getOptionalWeightVector(ov); + std::cerr << "["; + for (size_t j = 0; j < st->jpe_v[i]->numJoints(); j++) { + std::cerr << ov[j] << " "; + } + std::cerr << "]"; + } + std::cerr << std::endl; + std::cerr << "[" << m_profile.instance_name << "] sr_gains = ["; + for (size_t i = 0; i < st->jpe_v.size(); i++) { + std::cerr << st->jpe_v[i]->getSRGain() << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] avoid_gains = ["; + for (size_t i = 0; i < st->stikp.size(); i++) { + std::cerr << st->stikp[i].avoid_gain << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] reference_gains = ["; + for (size_t i = 0; i < st->stikp.size(); i++) { + std::cerr << st->stikp[i].reference_gain << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] manipulability_limits = ["; + for (size_t i = 0; i < st->jpe_v.size(); i++) { + std::cerr << st->jpe_v[i]->getManipulabilityLimit() << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] ik_loop_count = ["; + for (size_t i = 0; i < st->stikp.size(); i++) { + std::cerr << st->stikp[i].ik_loop_count << ", "; + } + std::cerr << "]" << std::endl; + } + // joint servo control parameters + std::cerr << "[" << m_profile.instance_name << "] joint servo control parameters" << std::endl; + if (control_mode == MODE_IDLE) { + // !TORQUE -> TORQUE + if( st->joint_control_mode != OpenHRP::RobotHardwareService::TORQUE && i_param.joint_control_mode == OpenHRP::RobotHardwareService::TORQUE ){ + for(size_t i = 0; i < st->stikp.size(); i++) { + Stabilizer::STIKParam& ikp = st->stikp[i]; + ikp.eefm_pos_damping_gain *= 1000; + ikp.eefm_rot_damping_gain *= 1000; + } + st->eefm_swing_pos_damping_gain *= 1000; + st->eefm_swing_rot_damping_gain *= 1000; + } + // TORQUE -> !TORQUE + if( st->joint_control_mode == OpenHRP::RobotHardwareService::TORQUE && i_param.joint_control_mode != OpenHRP::RobotHardwareService::TORQUE ){ + for(size_t i = 0; i < st->stikp.size(); i++) { + Stabilizer::STIKParam& ikp = st->stikp[i]; + ikp.eefm_pos_damping_gain /= 1000; + ikp.eefm_rot_damping_gain /= 1000; + } + st->eefm_swing_pos_damping_gain /= 1000; + st->eefm_swing_rot_damping_gain /= 1000; + } + st->joint_control_mode = i_param.joint_control_mode; + std::cerr << "[" << m_profile.instance_name << "] joint_control_mode changed" << std::endl; + } else { + std::cerr << "[" << m_profile.instance_name << "] joint_control_mode cannot be changed during MODE_AIR or MODE_ST." << std::endl; + } + st->swing2landing_transition_time = i_param.swing2landing_transition_time; + st->landing_phase_time = i_param.landing_phase_time; + st->landing2support_transition_time = i_param.landing2support_transition_time; + st->support_phase_min_time = i_param.support_phase_min_time; + st->support2swing_transition_time = i_param.support2swing_transition_time; + bool is_joint_servo_control_parameter_valid_length = true; + if ( i_param.joint_servo_control_parameters.length() == st->stikp.size() ) { + for (size_t i = 0; i < st->stikp.size(); i++) { + const OpenHRP::AutoBalancerService::JointServoControlParameter& jscp = i_param.joint_servo_control_parameters[i]; + if ( st->stikp[i].support_pgain.size() == i_param.joint_servo_control_parameters[i].support_pgain.length() && + st->stikp[i].support_dgain.size() == i_param.joint_servo_control_parameters[i].support_dgain.length() && + st->stikp[i].landing_pgain.size() == i_param.joint_servo_control_parameters[i].landing_pgain.length() && + st->stikp[i].landing_dgain.size() == i_param.joint_servo_control_parameters[i].landing_dgain.length() && + st->stikp[i].swing_pgain.size() == i_param.joint_servo_control_parameters[i].swing_pgain.length() && + st->stikp[i].swing_dgain.size() == i_param.joint_servo_control_parameters[i].swing_dgain.length() ) { + for (size_t j = 0; j < st->stikp[i].support_pgain.size(); j++) { + st->stikp[i].support_pgain(j) = i_param.joint_servo_control_parameters[i].support_pgain[j]; + st->stikp[i].support_dgain(j) = i_param.joint_servo_control_parameters[i].support_dgain[j]; + st->stikp[i].landing_pgain(j) = i_param.joint_servo_control_parameters[i].landing_pgain[j]; + st->stikp[i].landing_dgain(j) = i_param.joint_servo_control_parameters[i].landing_dgain[j]; + st->stikp[i].swing_pgain(j) = i_param.joint_servo_control_parameters[i].swing_pgain[j]; + st->stikp[i].swing_dgain(j) = i_param.joint_servo_control_parameters[i].swing_dgain[j]; + } + } else is_joint_servo_control_parameter_valid_length = false; + } + } else { + is_joint_servo_control_parameter_valid_length = false; + } + if ( is_joint_servo_control_parameter_valid_length ) { + std::cerr << "[" << m_profile.instance_name << "] support_pgain = ["; + for (size_t i = 0; i < st->stikp.size(); i++) std::cerr << "[" << st->stikp[i].support_pgain.transpose() << "],"; + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] support_dgain = ["; + for (size_t i = 0; i < st->stikp.size(); i++) std::cerr << "[" << st->stikp[i].support_dgain.transpose() << "],"; + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] landing_pgain = ["; + for (size_t i = 0; i < st->stikp.size(); i++) std::cerr << "[" << st->stikp[i].landing_pgain.transpose() << "],"; + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] landing_dgain = ["; + for (size_t i = 0; i < st->stikp.size(); i++) std::cerr << "[" << st->stikp[i].landing_dgain.transpose() << "],"; + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] swing_pgain = ["; + for (size_t i = 0; i < st->stikp.size(); i++) std::cerr << "[" << st->stikp[i].swing_pgain.transpose() << "],"; + std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] swing_dgain = ["; + for (size_t i = 0; i < st->stikp.size(); i++) std::cerr << "[" << st->stikp[i].swing_dgain.transpose() << "],"; + std::cerr << "]" << std::endl; + } else std::cerr << "[" << m_profile.instance_name << "] servo gain parameters cannot be set because of invalid param." << std::endl; +} + +void AutoBalancer::getStabilizerParam(OpenHRP::AutoBalancerService::StabilizerParam& i_param) +{ + std::cerr << "[" << m_profile.instance_name << "] getStabilizerParam" << std::endl; + for (size_t i = 0; i < 2; i++) { + // i_param.k_run_b[i] = k_run_b[i]; + // i_param.d_run_b[i] = d_run_b[i]; + //m_tau_x[i].setup(i_param.tdfke[0], i_param.tdftc[0], dt); + //m_tau_y[i].setup(i_param.tdfke[0], i_param.tdftc[0], dt); + //m_f_z.setup(i_param.tdfke[1], i_param.tdftc[1], dt); + i_param.k_tpcc_p[i] = st->k_tpcc_p[i]; + i_param.k_tpcc_x[i] = st->k_tpcc_x[i]; + i_param.k_brot_p[i] = st->k_brot_p[i]; + i_param.k_brot_tc[i] = st->k_brot_tc[i]; + } + // i_param.k_run_x = m_torque_k[0]; + // i_param.k_run_y = m_torque_k[1]; + // i_param.d_run_x = m_torque_d[0]; + // i_param.d_run_y = m_torque_d[1]; + for (size_t i = 0; i < 2; i++) { + i_param.eefm_k1[i] = st->eefm_k1[i]; + i_param.eefm_k2[i] = st->eefm_k2[i]; + i_param.eefm_k3[i] = st->eefm_k3[i]; + i_param.eefm_zmp_delay_time_const[i] = st->eefm_zmp_delay_time_const[i]; + i_param.eefm_ref_zmp_aux[i] = st->ref_zmp_aux(i); + i_param.eefm_body_attitude_control_time_const[i] = st->eefm_body_attitude_control_time_const[i]; + i_param.eefm_body_attitude_control_gain[i] = st->eefm_body_attitude_control_gain[i]; + i_param.ref_capture_point[i] = st->ref_cp(i); + i_param.act_capture_point[i] = st->act_cp(i); + i_param.cp_offset[i] = st->cp_offset(i); + } + i_param.eefm_pos_time_const_support.length(st->stikp.size()); + i_param.eefm_pos_damping_gain.length(st->stikp.size()); + i_param.eefm_pos_compensation_limit.length(st->stikp.size()); + i_param.eefm_swing_pos_spring_gain.length(st->stikp.size()); + i_param.eefm_swing_pos_time_const.length(st->stikp.size()); + i_param.eefm_rot_time_const.length(st->stikp.size()); + i_param.eefm_rot_damping_gain.length(st->stikp.size()); + i_param.eefm_rot_compensation_limit.length(st->stikp.size()); + i_param.eefm_swing_rot_spring_gain.length(st->stikp.size()); + i_param.eefm_swing_rot_time_const.length(st->stikp.size()); + i_param.eefm_ee_moment_limit.length(st->stikp.size()); + i_param.eefm_ee_forcemoment_distribution_weight.length(st->stikp.size()); + for (size_t j = 0; j < st->stikp.size(); j++) { + i_param.eefm_pos_damping_gain[j].length(3); + i_param.eefm_pos_time_const_support[j].length(3); + i_param.eefm_swing_pos_spring_gain[j].length(3); + i_param.eefm_swing_pos_time_const[j].length(3); + i_param.eefm_rot_damping_gain[j].length(3); + i_param.eefm_rot_time_const[j].length(3); + i_param.eefm_swing_rot_spring_gain[j].length(3); + i_param.eefm_swing_rot_time_const[j].length(3); + i_param.eefm_ee_moment_limit[j].length(3); + i_param.eefm_ee_forcemoment_distribution_weight[j].length(6); + for (size_t i = 0; i < 3; i++) { + i_param.eefm_pos_damping_gain[j][i] = st->stikp[j].eefm_pos_damping_gain(i); + i_param.eefm_pos_time_const_support[j][i] = st->stikp[j].eefm_pos_time_const_support(i); + i_param.eefm_swing_pos_spring_gain[j][i] = st->stikp[j].eefm_swing_pos_spring_gain(i); + i_param.eefm_swing_pos_time_const[j][i] = st->stikp[j].eefm_swing_pos_time_const(i); + i_param.eefm_rot_damping_gain[j][i] = st->stikp[j].eefm_rot_damping_gain(i); + i_param.eefm_rot_time_const[j][i] = st->stikp[j].eefm_rot_time_const(i); + i_param.eefm_swing_rot_spring_gain[j][i] = st->stikp[j].eefm_swing_rot_spring_gain(i); + i_param.eefm_swing_rot_time_const[j][i] = st->stikp[j].eefm_swing_rot_time_const(i); + i_param.eefm_ee_moment_limit[j][i] = st->stikp[j].eefm_ee_moment_limit(i); + i_param.eefm_ee_forcemoment_distribution_weight[j][i] = st->stikp[j].eefm_ee_forcemoment_distribution_weight(i); + i_param.eefm_ee_forcemoment_distribution_weight[j][i+3] = st->stikp[j].eefm_ee_forcemoment_distribution_weight(i+3); + } + i_param.eefm_pos_compensation_limit[j] = st->stikp[j].eefm_pos_compensation_limit; + i_param.eefm_rot_compensation_limit[j] = st->stikp[j].eefm_rot_compensation_limit; + } + for (size_t i = 0; i < 3; i++) { + i_param.eefm_swing_pos_damping_gain[i] = st->eefm_swing_pos_damping_gain(i); + i_param.eefm_swing_rot_damping_gain[i] = st->eefm_swing_rot_damping_gain(i); + } + i_param.eefm_pos_time_const_swing = st->eefm_pos_time_const_swing; + i_param.eefm_pos_transition_time = st->eefm_pos_transition_time; + i_param.eefm_pos_margin_time = st->eefm_pos_margin_time; + i_param.eefm_leg_inside_margin = st->szd->get_leg_inside_margin(); + i_param.eefm_leg_outside_margin = st->szd->get_leg_outside_margin(); + i_param.eefm_leg_front_margin = st->szd->get_leg_front_margin(); + i_param.eefm_leg_rear_margin = st->szd->get_leg_rear_margin(); + + std::vector > support_polygon_vec; + st->szd->get_vertices(support_polygon_vec); + i_param.eefm_support_polygon_vertices_sequence.length(support_polygon_vec.size()); + for (size_t ee_idx = 0; ee_idx < support_polygon_vec.size(); ee_idx++) { + i_param.eefm_support_polygon_vertices_sequence[ee_idx].vertices.length(support_polygon_vec[ee_idx].size()); + for (size_t v_idx = 0; v_idx < support_polygon_vec[ee_idx].size(); v_idx++) { + i_param.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[0] = support_polygon_vec[ee_idx][v_idx](0); + i_param.eefm_support_polygon_vertices_sequence[ee_idx].vertices[v_idx].pos[1] = support_polygon_vec[ee_idx][v_idx](1); + } + } + + i_param.eefm_cogvel_cutoff_freq = st->act_cogvel_filter->getCutOffFreq(); + i_param.eefm_wrench_alpha_blending = st->szd->get_wrench_alpha_blending(); + i_param.eefm_alpha_cutoff_freq = st->szd->get_alpha_cutoff_freq(); + i_param.eefm_gravitational_acceleration = st->eefm_gravitational_acceleration; + i_param.eefm_ee_error_cutoff_freq = st->stikp[0].target_ee_diff_p_filter->getCutOffFreq(); + i_param.eefm_use_force_difference_control = st->eefm_use_force_difference_control; + i_param.eefm_use_swing_damping = st->eefm_use_swing_damping; + for (size_t i = 0; i < 3; ++i) { + i_param.eefm_swing_damping_force_thre[i] = st->eefm_swing_damping_force_thre[i]; + i_param.eefm_swing_damping_moment_thre[i] = st->eefm_swing_damping_moment_thre[i]; + } + i_param.is_ik_enable.length(st->is_ik_enable.size()); + for (size_t i = 0; i < st->is_ik_enable.size(); i++) { + i_param.is_ik_enable[i] = st->is_ik_enable[i]; + } + i_param.is_feedback_control_enable.length(st->is_feedback_control_enable.size()); + for (size_t i = 0; i < st->is_feedback_control_enable.size(); i++) { + i_param.is_feedback_control_enable[i] = st->is_feedback_control_enable[i]; + } + i_param.is_zmp_calc_enable.length(st->is_zmp_calc_enable.size()); + for (size_t i = 0; i < st->is_zmp_calc_enable.size(); i++) { + i_param.is_zmp_calc_enable[i] = st->is_zmp_calc_enable[i]; + } + + i_param.foot_origin_offset.length(2); + for (size_t i = 0; i < i_param.foot_origin_offset.length(); i++) { + i_param.foot_origin_offset[i].length(3); + i_param.foot_origin_offset[i][0] = st->foot_origin_offset[i](0); + i_param.foot_origin_offset[i][1] = st->foot_origin_offset[i](1); + i_param.foot_origin_offset[i][2] = st->foot_origin_offset[i](2); + } + i_param.st_algorithm = st->st_algorithm; + i_param.transition_time = st->transition_time; + i_param.cop_check_margin = st->cop_check_margin; + for (size_t i = 0; i < st->cp_check_margin.size(); i++) { + i_param.cp_check_margin[i] = st->cp_check_margin[i]; + } + for (size_t i = 0; i < st->tilt_margin.size(); i++) { + i_param.tilt_margin[i] = st->tilt_margin[i]; + } + i_param.contact_decision_threshold = st->contact_decision_threshold; + i_param.is_estop_while_walking = st->is_estop_while_walking; + switch(st->control_mode) { + case Stabilizer::MODE_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_STIDLE; break; + case Stabilizer::MODE_AIR: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_AIR; break; + case Stabilizer::MODE_ST: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_ST; break; + case Stabilizer::MODE_SYNC_TO_IDLE: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_STIDLE; break; + case Stabilizer::MODE_SYNC_TO_AIR: i_param.controller_mode = OpenHRP::AutoBalancerService::MODE_SYNC_TO_AIR; break; + default: break; + } + i_param.emergency_check_mode = st->emergency_check_mode; + i_param.end_effector_list.length(st->stikp.size()); + i_param.use_limb_stretch_avoidance = st->use_limb_stretch_avoidance; + i_param.use_zmp_truncation = st->use_zmp_truncation; + i_param.use_force_sensor = st->use_force_sensor; + i_param.use_footguided_stabilizer = st->use_footguided_stabilizer; + i_param.footguided_balance_time_const = st->footguided_balance_time_const; + i_param.limb_stretch_avoidance_time_const = st->limb_stretch_avoidance_time_const; + i_param.limb_length_margin.length(st->stikp.size()); + i_param.detection_time_to_air = st->detection_count_to_air * m_dt; + for (size_t i = 0; i < 2; i++) { + i_param.limb_stretch_avoidance_vlimit[i] = st->limb_stretch_avoidance_vlimit[i]; + i_param.root_rot_compensation_limit[i] = st->root_rot_compensation_limit[i]; + } + for (size_t i = 0; i < st->stikp.size(); i++) { + const rats::coordinates cur_ee = rats::coordinates(st->stikp.at(i).localp, st->stikp.at(i).localR); + OpenHRP::AutoBalancerService::Footstep ret_ee; + // position + memcpy(ret_ee.pos, cur_ee.pos.data(), sizeof(double)*3); + // rotation + Eigen::Quaternion qt(cur_ee.rot); + ret_ee.rot[0] = qt.w(); + ret_ee.rot[1] = qt.x(); + ret_ee.rot[2] = qt.y(); + ret_ee.rot[3] = qt.z(); + // name + ret_ee.leg = st->stikp.at(i).ee_name.c_str(); + // set + i_param.end_effector_list[i] = ret_ee; + i_param.limb_length_margin[i] = st->stikp[i].limb_length_margin; + } + i_param.ik_limb_parameters.length(st->jpe_v.size()); + for (size_t i = 0; i < st->jpe_v.size(); i++) { + OpenHRP::AutoBalancerService::IKLimbParameters& ilp = i_param.ik_limb_parameters[i]; + ilp.ik_optional_weight_vector.length(st->jpe_v[i]->numJoints()); + std::vector ov; + ov.resize(st->jpe_v[i]->numJoints()); + st->jpe_v[i]->getOptionalWeightVector(ov); + for (size_t j = 0; j < st->jpe_v[i]->numJoints(); j++) { + ilp.ik_optional_weight_vector[j] = ov[j]; + } + ilp.sr_gain = st->jpe_v[i]->getSRGain(); + ilp.avoid_gain = st->stikp[i].avoid_gain; + ilp.reference_gain = st->stikp[i].reference_gain; + ilp.manipulability_limit = st->jpe_v[i]->getManipulabilityLimit(); + ilp.ik_loop_count = st->stikp[i].ik_loop_count; // size_t -> unsigned short, value may change, but ik_loop_count is small value and value not change + } + i_param.swing2landing_transition_time = st->swing2landing_transition_time; + i_param.landing_phase_time = st->landing_phase_time; + i_param.landing2support_transition_time = st->landing2support_transition_time; + i_param.support_phase_min_time = st->support_phase_min_time; + i_param.support2swing_transition_time = st->support2swing_transition_time; + i_param.joint_control_mode = st->joint_control_mode; + i_param.joint_servo_control_parameters.length(st->stikp.size()); + for (size_t i = 0; i < st->stikp.size(); i++) { + OpenHRP::AutoBalancerService::JointServoControlParameter& jscp = i_param.joint_servo_control_parameters[i]; + jscp.support_pgain.length(st->stikp[i].support_pgain.size()); + jscp.support_dgain.length(st->stikp[i].support_dgain.size()); + jscp.landing_pgain.length(st->stikp[i].landing_pgain.size()); + jscp.landing_dgain.length(st->stikp[i].landing_dgain.size()); + jscp.swing_pgain.length(st->stikp[i].swing_pgain.size()); + jscp.swing_dgain.length(st->stikp[i].swing_dgain.size()); + for (size_t j=0; j < st->stikp[i].support_pgain.size(); j++) { + jscp.support_pgain[j] = st->stikp[i].support_pgain(j); + jscp.support_dgain[j] = st->stikp[i].support_dgain(j); + jscp.landing_pgain[j] = st->stikp[i].landing_pgain(j); + jscp.landing_dgain[j] = st->stikp[i].landing_dgain(j); + jscp.swing_pgain[j] = st->stikp[i].swing_pgain(j); + jscp.swing_dgain[j] = st->stikp[i].swing_dgain(j); + } + } +} + void AutoBalancer::copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs) { memcpy(out_fs.pos, in_fs.pos.data(), sizeof(double)*3); @@ -1785,7 +3974,8 @@ bool AutoBalancer::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam& bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep) { - std::cerr << "[" << m_profile.instance_name << "] adjustFootSteps" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] adjustFootSteps" << std::endl; if (control_mode == MODE_ABC && !gg_is_walking && adjust_footstep_interpolator->isEmpty()) { Guard guard(m_mutex); // @@ -1805,13 +3995,13 @@ bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& // Input : ee coords // Output : link coords memcpy(eepos.data(), rfootstep.pos, sizeof(double)*3); - eerot = (Eigen::Quaternion(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: - ikp["rleg"].adjust_interpolation_target_r0 = eerot * ikp["rleg"].localR.transpose(); - ikp["rleg"].adjust_interpolation_target_p0 = eepos - ikp["rleg"].adjust_interpolation_target_r0 * ikp["rleg"].localPos; + eerot = (Eigen::Quaternion(rfootstep.rot[0], rfootstep.rot[1], rfootstep.rot[2], rfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: + ikp["rleg"].adjust_interpolation_target_r0 = eerot; + ikp["rleg"].adjust_interpolation_target_p0 = eepos; memcpy(eepos.data(), lfootstep.pos, sizeof(double)*3); - eerot = (Eigen::Quaternion(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: - ikp["lleg"].adjust_interpolation_target_r0 = eerot * ikp["lleg"].localR.transpose(); - ikp["lleg"].adjust_interpolation_target_p0 = eepos - ikp["lleg"].adjust_interpolation_target_r0 * ikp["lleg"].localPos; + eerot = (Eigen::Quaternion(lfootstep.rot[0], lfootstep.rot[1], lfootstep.rot[2], lfootstep.rot[3])).normalized().toRotationMatrix(); // rtc: + ikp["lleg"].adjust_interpolation_target_r0 = eerot; + ikp["lleg"].adjust_interpolation_target_p0 = eepos; mid_coords(target_mid_coords, 0.5, coordinates(ikp["rleg"].adjust_interpolation_target_p0, ikp["rleg"].adjust_interpolation_target_r0), coordinates(ikp["lleg"].adjust_interpolation_target_p0, ikp["lleg"].adjust_interpolation_target_r0)); @@ -1838,7 +4028,7 @@ bool AutoBalancer::adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& double tmp = 0.0; adjust_footstep_interpolator->set(&tmp); tmp = 1.0; - adjust_footstep_interpolator->go(&tmp, adjust_footstep_transition_time, true); + adjust_footstep_interpolator->setGoal(&tmp, adjust_footstep_transition_time, true); } while (!adjust_footstep_interpolator->isEmpty() ) usleep(1000); @@ -1859,6 +4049,7 @@ bool AutoBalancer::getRemainingFootstepSequence(OpenHRP::AutoBalancerService::Fo copyRatscoords2Footstep(o_footstep[i], fsnl[i].front().worldcoords); } } + return true; }; bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep) @@ -1899,42 +4090,75 @@ bool AutoBalancer::getGoPosFootstepsSequence(const double& x, const double& y, c void AutoBalancer::static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height) { hrp::Vector3 target_sbp = hrp::Vector3(0, 0, 0); - hrp::Vector3 tmpcog = m_robot->calcCM(); - switch ( use_force ) { - case MODE_REF_FORCE: - calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height, ref_forces); + hrp::Vector3 tmpcog; + if (!gg_is_walking || !use_act_states) { + tmpcog = m_robot->calcCM(); + dif_ref_act_cog = hrp::Vector3::Zero(); + } else { + tmpcog = st->ref_foot_origin_pos + st->ref_foot_origin_rot * st->act_cog; + dif_ref_act_cog = m_robot->calcCM() - tmpcog; + } + if ( use_force == MODE_NO_FORCE ) { + tmp_input_sbp = tmpcog + sbp_cog_offset; + } else { + calc_static_balance_point_from_forces(target_sbp, tmpcog, ref_com_height); tmp_input_sbp = target_sbp - sbp_offset; sbp_cog_offset = tmp_input_sbp - tmpcog; - break; - case MODE_NO_FORCE: - tmp_input_sbp = tmpcog + sbp_cog_offset; - break; - default: break; } }; -void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector& tmp_forces) +void AutoBalancer::calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height) { hrp::Vector3 denom, nume; /* sb_point[m] = nume[kg * m/s^2 * m] / denom[kg * m/s^2] */ double mass = m_robot->totalMass(); + double mg = mass * gg->get_gravitational_acceleration(); + hrp::Vector3 total_sensor_ref_force = hrp::Vector3::Zero(); + for (size_t i = 0; i < ref_forces.size(); i++) { + total_sensor_ref_force += ref_forces[i]; + } + hrp::Vector3 total_nosensor_ref_force = mg * hrp::Vector3::UnitZ() - total_sensor_ref_force; // total ref force at the point without sensors, such as torso + hrp::Vector3 tmp_ext_moment = fix_leg_coords2.pos.cross(total_nosensor_ref_force) + fix_leg_coords2.rot * hrp::Vector3(m_refFootOriginExtMoment.data.x, m_refFootOriginExtMoment.data.y, m_refFootOriginExtMoment.data.z); + // For MODE_REF_FORCE_RFU_EXT_MOMENT, store previous root position to calculate influence from tmp_ext_moment while walking (basically, root link moves while walking). + // Calculate values via fix_leg_coords2 relative/world values. + static hrp::Vector3 prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos); + // If not is_hold_value (not hold value), update prev_additional_force_applied_pos + if ( !m_refFootOriginExtMomentIsHoldValue.data ) { + prev_additional_force_applied_pos = fix_leg_coords2.rot.transpose() * (additional_force_applied_link->p-fix_leg_coords2.pos); + } + hrp::Vector3 tmp_prev_additional_force_applied_pos = fix_leg_coords2.rot * prev_additional_force_applied_pos + fix_leg_coords2.pos; + // Calculate SBP for (size_t j = 0; j < 2; j++) { - nume(j) = mass * gg->get_gravitational_acceleration() * tmpcog(j); - denom(j) = mass * gg->get_gravitational_acceleration(); - for (size_t i = 0; i < sensor_names.size(); i++) { - if ( sensor_names[i].find("hsensor") != std::string::npos || sensor_names[i].find("asensor") != std::string::npos ) { // tempolary to get arm force coords - hrp::Link* parentlink; - hrp::ForceSensor* sensor = m_robot->sensor(sensor_names[i]); - if (sensor) parentlink = sensor->link; - else parentlink = m_vfs[sensor_names[i]].link; - for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { - if (it->second.target_link->name == parentlink->name) { - hrp::Vector3 fpos = parentlink->p + parentlink->R * it->second.localPos; - nume(j) += ( (fpos(2) - ref_com_height) * tmp_forces[i](j) - fpos(j) * tmp_forces[i](2) ); - denom(j) -= tmp_forces[i](2); - } - } - } + nume(j) = mg * tmpcog(j); + denom(j) = mg; + if ( use_force == MODE_REF_FORCE_RFU_EXT_MOMENT ) { + //nume(j) += (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0)); + nume(j) += (tmp_prev_additional_force_applied_pos(j)-additional_force_applied_link->p(j))*total_nosensor_ref_force(2) + (j==0 ? tmp_ext_moment(1):-tmp_ext_moment(0)); + denom(j) -= total_nosensor_ref_force(2); + } else { + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + // Check leg_names. leg_names is assumed to be support limb for locomotion, cannot be used for manipulation. If it->first is not included in leg_names, use it for manipulation and static balance point calculation. + if (std::find(leg_names.begin(), leg_names.end(), it->first) == leg_names.end()) { + size_t idx = contact_states_index_map[it->first]; + // Force applied point is assumed as end effector + hrp::Vector3 fpos = it->second.target_link->p + it->second.target_link->R * it->second.localPos; + nume(j) += ( (fpos(2) - ref_com_height) * ref_forces[idx](j) - fpos(j) * ref_forces[idx](2) ); + nume(j) += (j==0 ? ref_moments[idx](1):-ref_moments[idx](0)); + denom(j) -= ref_forces[idx](2); + } + } + if ( use_force == MODE_REF_FORCE_WITH_FOOT ) { + hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset); + nume(j) += ( (fpos(2) - ref_com_height) * total_nosensor_ref_force(j) - fpos(j) * total_nosensor_ref_force(2) ); + denom(j) -= total_nosensor_ref_force(2); + } + if ( use_force == MODE_ADDITIONAL_FORCE ) { + size_t idx = ref_forces.size() - 1; + hrp::Vector3 fpos(additional_force_applied_link->p+additional_force_applied_point_offset); + nume(j) += ( (fpos(2) - ref_com_height) * ref_forces[idx](j) - fpos(j) * ref_forces[idx](2) ); + nume(j) += (j==0 ? ref_moments[idx](1):-ref_moments[idx](0)); + denom(j) -= ref_forces[idx](2); + } } sb_point(j) = nume(j) / denom(j); } @@ -1956,8 +4180,8 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_ ref_hand_coords.transform(graspless_manip_reference_trans_coords); // desired arm coords hrp::Vector3 foot_pos(gg->get_dst_foot_midcoords().pos); if ( graspless_manip_arm == "arms" ) { - hrp::Vector3 rarm_pos = ikp["rarm"].target_p0 + ikp["rarm"].target_r0 * ikp["rarm"].localPos; - hrp::Vector3 larm_pos = ikp["larm"].target_p0 + ikp["larm"].target_r0 * ikp["larm"].localPos; + hrp::Vector3 rarm_pos = ikp["rarm"].target_p0; + hrp::Vector3 larm_pos = ikp["larm"].target_p0; act_hand_coords.pos = (rarm_pos+larm_pos)/2.0; hrp::Vector3 act_y = larm_pos-rarm_pos; act_y(2) = 0; @@ -1968,8 +4192,8 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_ dr = hrp::Vector3(0,0,(hrp::Vector3(ref_y.cross(act_y))(2) > 0 ? 1.0 : -1.0) * std::acos(ref_y.dot(act_y))); // fix for rotation } else { ABCIKparam& tmpikp = ikp[graspless_manip_arm]; - act_hand_coords = rats::coordinates(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos, - tmpikp.target_r0 * tmpikp.localR); + act_hand_coords = rats::coordinates(tmpikp.target_p0, + tmpikp.target_r0); rats::difference_rotation(dr, ref_hand_coords.rot, act_hand_coords.rot); dr(0) = 0; dr(1) = 0; } @@ -1991,22 +4215,23 @@ hrp::Vector3 AutoBalancer::calc_vel_from_hand_error (const coordinates& tmp_fix_ bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector& initial_support_legs_coords, std::vector& initial_support_legs, coordinates& start_ref_coords) { switch(gait_type) { case BIPED: - initial_support_legs_coords = (y > 0 ? - boost::assign::list_of(ikp["rleg"].target_end_coords) - : boost::assign::list_of(ikp["lleg"].target_end_coords)); - initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG) : boost::assign::list_of(LLEG)); + initial_support_legs_coords.assign (1, + y > 0 ? + coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0) + : coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0)); + initial_support_legs.assign (1, y > 0 ? RLEG : LLEG); break; case TROT: initial_support_legs_coords = (y > 0 ? - boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["larm"].target_end_coords) - : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["rarm"].target_end_coords)); - initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)); + boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0)) + : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0))).convert_to_container < std::vector > (); + initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(LARM) : boost::assign::list_of(LLEG)(RARM)).convert_to_container < std::vector > (); break; case PACE: initial_support_legs_coords = (y > 0 ? - boost::assign::list_of(ikp["rleg"].target_end_coords)(ikp["rarm"].target_end_coords) - : boost::assign::list_of(ikp["lleg"].target_end_coords)(ikp["larm"].target_end_coords)); - initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)); + boost::assign::list_of(coordinates(ikp["rleg"].target_p0, ikp["rleg"].target_r0))(coordinates(ikp["rarm"].target_p0, ikp["rarm"].target_r0)) + : boost::assign::list_of(coordinates(ikp["lleg"].target_p0, ikp["lleg"].target_r0))(coordinates(ikp["larm"].target_p0, ikp["larm"].target_r0))).convert_to_container < std::vector > (); + initial_support_legs = (y > 0 ? boost::assign::list_of(RLEG)(RARM) : boost::assign::list_of(LLEG)(LARM)).convert_to_container < std::vector > (); break; case CRAWL: std::cerr << "[" << m_profile.instance_name << "] crawl walk[" << gait_type << "] is not implemented yet." << std::endl; @@ -2017,10 +4242,144 @@ bool AutoBalancer::calc_inital_support_legs(const double& y, std::vector x = W A^T (A W A^T)-1 b +// => x = W^{1/2} Pinv(A W^{1/2}) b +// Copied from ZMPDistributor.h +void calcWeightedLinearEquation(hrp::dvector& ret, const hrp::dmatrix& A, const hrp::dmatrix& W, const hrp::dvector& b) +{ + hrp::dmatrix W2 = hrp::dmatrix::Zero(W.rows(), W.cols()); + for (size_t i = 0; i < W.rows(); i++) W2(i,i) = std::sqrt(W(i,i)); + hrp::dmatrix Aw = A*W2; + hrp::dmatrix Aw_inv = hrp::dmatrix::Zero(A.cols(), A.rows()); + hrp::calcPseudoInverse(Aw, Aw_inv); + ret = W2 * Aw_inv * b; + //ret = W2 * Aw.colPivHouseholderQr().solve(b); +}; + +void AutoBalancer::distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zmp) +{ + // apply inverse system + // TODO : fix 0.055 (zmp delay) + hrp::Vector3 tmp_ref_zmp = _ref_zmp + 0.055 * (_ref_zmp - prev_ref_zmp) / m_dt; + + std::vector cop_pos; + std::vector limb_gains; + for (size_t i = 0 ; i < leg_names.size(); i++) { + ABCIKparam& tmpikp = ikp[leg_names[i]]; + cop_pos.push_back(tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localR * default_zmp_offsets[i]); + limb_gains.push_back(m_contactStates.data[contact_states_index_map[leg_names[i]]] ? 1.0 : 0.0); + } + size_t ee_num = leg_names.size(); + size_t state_dim = 6*ee_num; + size_t total_wrench_dim = 5; + // size_t total_fz = m_robot->totalMass() * gg->get_gravitational_acceleration(); + size_t total_fz = m_ref_force[0].data[2]+m_ref_force[1].data[2]; + //size_t total_wrench_dim = 3; + hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2); + hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim/2); + // Set Gmat + // Fill Fz + for (size_t j = 0; j < ee_num; j++) { + if (total_wrench_dim == 3) { + Gmat(0,3*j+2) = 1.0; + } else { + for (size_t k = 0; k < 3; k++) Gmat(k,3*j+k) = 1.0; + } + } + // Fill Nx and Ny + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < ee_num; j++) { + if ( i == total_wrench_dim-2 ) { // Nx + Gmat(i,3*j+1) = -(cop_pos[j](2) - tmp_ref_zmp(2)); + Gmat(i,3*j+2) = (cop_pos[j](1) - tmp_ref_zmp(1)); + } else if ( i == total_wrench_dim-1 ) { // Ny + Gmat(i,3*j) = (cop_pos[j](2) - tmp_ref_zmp(2)); + Gmat(i,3*j+2) = -(cop_pos[j](0) - tmp_ref_zmp(0)); + } + } + } + // Set Wmat + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + if (ee_num == 2) + Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j] * (i==2? 1.0 : 0.01); + else + Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j]; + } + } + // Ret is wrench around cop_pos + // f_cop = f_ee + // n_ee = (cop_pos - ee_pos) x f_cop + n_cop + hrp::dvector ret(state_dim/2); + hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim); + total_wrench(total_wrench_dim-5) = m_ref_force[0].data[0]+m_ref_force[1].data[0]; + total_wrench(total_wrench_dim-4) = m_ref_force[0].data[1]+m_ref_force[1].data[1]; + total_wrench(total_wrench_dim-3) = total_fz; + calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench); + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] distributeReferenceZMPToWrenches" << std::endl; + } + for (size_t i = 0 ; i < leg_names.size(); i++) { + size_t fidx = contact_states_index_map[leg_names[i]]; + ABCIKparam& tmpikp = ikp[leg_names[i]]; + hrp::Vector3 f_ee(ret(3*i), ret(3*i+1), ret(3*i+2)); + //hrp::Vector3 tmp_ee_pos = tmpikp.target_p0 + tmpikp.target_r0 * tmpikp.localPos; + hrp::Vector3 tmp_ee_pos = tmpikp.target_p0; + hrp::Vector3 n_ee = (cop_pos[i]-tmp_ee_pos).cross(f_ee); // n_cop = 0 + m_force[fidx].data[0] = f_ee(0); + m_force[fidx].data[1] = f_ee(1); + m_force[fidx].data[2] = f_ee(2); + m_force[fidx].data[3] = n_ee(0); + m_force[fidx].data[4] = n_ee(1); + m_force[fidx].data[5] = n_ee(2); + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] " + << "ref_force [" << leg_names[i] << "] " << f_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "ref_moment [" << leg_names[i] << "] " << n_ee.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + } + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] Gmat = " << Gmat.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] total_wrench = " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl; + hrp::dvector tmp(total_wrench.size()); + tmp = Gmat*ret; + std::cerr << "[" << m_profile.instance_name << "] Gmat*ret = " << tmp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] (Gmat*ret-total_wrench) = " << (tmp-total_wrench).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] ret = " << ret.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N,Nm]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Wmat(diag) = ["; + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + std::cerr << Wmat(i+j*3, i+j*3) << " "; + } + } + std::cerr << "]" << std::endl; + } +}; + +std::string AutoBalancer::getUseForceModeString () +{ + switch (use_force) { + case OpenHRP::AutoBalancerService::MODE_NO_FORCE: + return "MODE_NO_FORCE"; + case OpenHRP::AutoBalancerService::MODE_REF_FORCE: + return "MODE_REF_FORCE"; + case OpenHRP::AutoBalancerService::MODE_REF_FORCE_WITH_FOOT: + return "MODE_REF_FORCE_WITH_FOOT"; + case OpenHRP::AutoBalancerService::MODE_REF_FORCE_RFU_EXT_MOMENT: + return "MODE_REF_FORCE_RFU_EXT_MOMENT"; + case OpenHRP::AutoBalancerService::MODE_ADDITIONAL_FORCE: + return "MODE_ADDITIONAL_FORCE"; + default: + return ""; + } +}; + // extern "C" { @@ -2034,5 +4393,3 @@ extern "C" } }; - - diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index e2c03e6e2e1..29d8ac7a4ce 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -10,6 +10,8 @@ #ifndef AUTOBALANCER_H #define AUTOBALANCER_H +#include +#include #include #include #include @@ -21,10 +23,14 @@ #include "../ImpedanceController/JointPathEx.h" #include "../ImpedanceController/RatsMatrix.h" #include "GaitGenerator.h" +#include "Stabilizer.h" // Service implementation headers // #include "AutoBalancerService_impl.h" #include "interpolator.h" +#include "../TorqueFilter/IIRFilter.h" +//#include "SimpleFullbodyInverseKinematicsSolver.h" +#include "FullbodyInverseKinematicsSolver.h" // @@ -90,13 +96,18 @@ class AutoBalancer // no corresponding operation exists in OpenRTm-aist-0.2.0 // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); bool goPos(const double& x, const double& y, const double& th); + bool goPosWheel(const double& x, const double& y, const double& th, const double w_x, const double rv_max, const double ra_max, const double tm_off); + bool goWheel(const double& x, const double& rv_max, const double& ra_max); + bool startWheeling(); bool goVelocity(const double& vx, const double& vy, const double& vth); bool goStop(); + bool jumpTo(const double& x, const double& y, const double& z, const double& ts, const double& tf); bool emergencyStop (); bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepSequence& fs, CORBA::Long overwrite_fs_idx); bool setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx); bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepSequence& fs, const OpenHRP::AutoBalancerService::StepParamSequence& sps, CORBA::Long overwrite_fs_idx); - bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx); + bool setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx, const bool is_wheel = false); + bool setFootStepsWithWheel(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, const OpenHRP::AutoBalancerService::WheelParamsSequence& wpss, CORBA::Long overwrite_fs_idx); void waitFootSteps(); void waitFootStepsEarly(const double tm); bool startAutoBalancer(const ::OpenHRP::AutoBalancerService::StrSequence& limbs); @@ -110,6 +121,14 @@ class AutoBalancer bool getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long& o_current_fs_idx); bool getGoPosFootstepsSequence(const double& x, const double& y, const double& th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep); bool releaseEmergencyStop(); + void distributeReferenceZMPToWrenches (const hrp::Vector3& _ref_zmp); + + // Stabilizer + void getStabilizerParam(OpenHRP::AutoBalancerService::StabilizerParam& i_param); + void setStabilizerParam(const OpenHRP::AutoBalancerService::StabilizerParam& i_param); + void startStabilizer(void); + void stopStabilizer(void); + protected: // Configuration variable declaration @@ -121,6 +140,10 @@ class AutoBalancer // TimedDoubleSeq m_qRef; InPort m_qRefIn; + TimedDoubleSeq m_qCurrent; + InPort m_qCurrentIn; + TimedDoubleSeq m_qTouchWall; + InPort m_qTouchWallIn; TimedPoint3D m_basePos; InPort m_basePosIn; TimedOrientation3D m_baseRpy; @@ -131,17 +154,52 @@ class AutoBalancer InPort m_optionalDataIn; std::vector m_ref_force; std::vector *> m_ref_forceIn; - TimedLong m_emergencySignal; - InPort m_emergencySignalIn; + TimedPoint3D m_diffCP; + InPort m_diffCPIn; + TimedPoint3D m_refFootOriginExtMoment; + InPort m_refFootOriginExtMomentIn; + TimedBoolean m_refFootOriginExtMomentIsHoldValue; + InPort m_refFootOriginExtMomentIsHoldValueIn; + TimedOrientation3D m_rpy; + InPort m_rpyIn; + TimedDoubleSeq m_qRefSeq; + InPort m_qRefSeqIn; + TimedBoolean m_touchWallMotionSolved; + InPort m_touchWallMotionSolvedIn; + std::vector m_wrenches; + std::vector *> m_wrenchesIn; + OpenHRP::TimedLandingPosition m_landingHeight; + InPort m_landingHeightIn; + OpenHRP::TimedSteppableRegion m_steppableRegion; + InPort m_steppableRegionIn; + // for debug TimedPoint3D m_cog; + TimedPoint3D m_originRefZmp, m_originRefCog, m_originRefCogVel, m_originNewZmp; + TimedPoint3D m_originActZmp, m_originActCog, m_originActCogVel; // // DataOutPort declaration // OutPort m_qOut; - RTC::OutPort m_zmpOut; + OutPort m_zmpOut; + TimedPoint3D m_zmpAct; + OutPort m_zmpActOut; + TimedPoint3D m_refCP; + OutPort m_refCPOut; + TimedPoint3D m_actCP; + OutPort m_actCPOut; + TimedDoubleSeq m_qAbc; + OutPort m_qAbcOut; + TimedDoubleSeq m_tmp; + OutPort m_tmpOut; + TimedPoint3D m_currentLandingPos; + OutPort m_currentLandingPosOut; + TimedPoint3D m_diffFootOriginExtMoment; + OutPort m_diffFootOriginExtMomentOut; + TimedDoubleSeq m_allEEComp; + OutPort m_allEECompOut; OutPort m_basePosOut; OutPort m_baseRpyOut; TimedDoubleSeq m_baseTform; @@ -152,24 +210,52 @@ class AutoBalancer OutPort m_accRefOut; TimedBooleanSeq m_contactStates; OutPort m_contactStatesOut; + TimedBooleanSeq m_actContactStates; + OutPort m_actContactStatesOut; + TimedDoubleSeq m_COPInfo; + OutPort m_COPInfoOut; + TimedDoubleSeq m_toeheelRatio; + OutPort m_toeheelRatioOut; TimedDoubleSeq m_controlSwingSupportTime; OutPort m_controlSwingSupportTimeOut; TimedBoolean m_walkingStates; OutPort m_walkingStatesOut; TimedPoint3D m_sbpCogOffset; OutPort m_sbpCogOffsetOut; + TimedLong m_emergencySignal; + OutPort m_emergencySignalOut; + TimedBoolean m_emergencyFallMotion; + OutPort m_emergencyFallMotionOut; + TimedBoolean m_isStuck; + OutPort m_isStuckOut; + TimedBoolean m_useFlywheel; + OutPort m_useFlywheelOut; + TimedPoint3D m_estimatedFxy; + OutPort m_estimatedFxyOut; + OpenHRP::TimedLandingPosition m_landingTarget; + OutPort m_landingTargetOut; + OpenHRP::TimedCogState m_endCogState; + OutPort m_endCogStateOut; std::vector m_force; std::vector *> m_ref_forceOut; std::vector m_limbCOPOffset; std::vector *> m_limbCOPOffsetOut; + TimedDoubleSeq m_tau; + OutPort m_tauOut; // for debug OutPort m_cogOut; + OutPort m_originRefZmpOut, m_originRefCogOut, m_originRefCogVelOut, m_originNewZmpOut; + OutPort m_originActZmpOut, m_originActCogOut, m_originActCogVelOut; + OpenHRP::TimedSteppableRegion m_currentSteppableRegion; + OutPort m_currentSteppableRegionOut; + // // CORBA Port declaration // RTC::CorbaPort m_AutoBalancerServicePort; + RTC::CorbaPort m_RobotHardwareServicePort; // @@ -186,55 +272,83 @@ class AutoBalancer private: struct ABCIKparam { - hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0; + hrp::Vector3 target_p0, localPos, adjust_interpolation_target_p0, adjust_interpolation_org_p0, handfix_target_p0; hrp::Matrix33 target_r0, localR, adjust_interpolation_target_r0, adjust_interpolation_org_r0; - rats::coordinates target_end_coords; hrp::Link* target_link; - hrp::JointPathExPtr manip; - double avoid_gain, reference_gain; - size_t pos_ik_error_count, rot_ik_error_count; bool is_active, has_toe_joint; }; - void getCurrentParameters(); void getTargetParameters(); - bool solveLimbIKforLimb (ABCIKparam& param); - void solveLimbIK(); + void solveSimpleFullbodyIK (); + void solveFullbodyIK (); void startABCparam(const ::OpenHRP::AutoBalancerService::StrSequence& limbs); void stopABCparam(); + void stopABCparamEmergency(); void waitABCTransition(); + // Functions to calculate parameters for ABC output. + // Output parameters are EE, limbCOPOffset, contactStates, controlSwingSupportTime, toeheelPhaseRatio + void getOutputParametersForWalking (); + void getOutputParametersForABC (); + void getOutputParametersForJumping (); + void getOutputParametersForWheeling (); + void getOutputParametersForIDLE (); + void interpolateLegNamesAndZMPOffsets(); + void calcFixCoordsForAdjustFootstep (rats::coordinates& tmp_fix_coords); + void rotateRefForcesForFixCoords (rats::coordinates& tmp_fix_coords); + void updateTargetCoordsForHandFixMode (rats::coordinates& tmp_fix_coords); + void calculateOutputRefForces (); + hrp::Vector3 calcFootMidPosUsingZMPWeightMap (); + void updateWalkingVelocityFromHandError (rats::coordinates& tmp_fix_coords); + void calcReferenceJointAnglesForIK (); hrp::Matrix33 OrientRotationMatrix (const hrp::Matrix33& rot, const hrp::Vector3& axis1, const hrp::Vector3& axis2); void fixLegToCoords (const hrp::Vector3& fix_pos, const hrp::Matrix33& fix_rot); - void startWalking (); + void fixLegToCoords2 (rats::coordinates& tmp_fix_coords); + bool startWalking (const bool is_wheel = false); void stopWalking (); + void stopJumping (); + void stopWheeling (); void copyRatscoords2Footstep(OpenHRP::AutoBalancerService::Footstep& out_fs, const rats::coordinates& in_fs); // static balance point offsetting void static_balance_point_proc_one(hrp::Vector3& tmp_input_sbp, const double ref_com_height); - void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height, std::vector& tmp_forces); + void calc_static_balance_point_from_forces(hrp::Vector3& sb_point, const hrp::Vector3& tmpcog, const double ref_com_height); hrp::Vector3 calc_vel_from_hand_error (const rats::coordinates& tmp_fix_coords); bool isOptionalDataContact (const std::string& ee_name) { return (std::fabs(m_optionalData.data[contact_states_index_map[ee_name]]-1.0)<0.1)?true:false; }; bool calc_inital_support_legs(const double& y, std::vector& initial_support_legs_coords, std::vector& initial_support_legs, rats::coordinates& start_ref_coords); + std::string getUseForceModeString (); + void setActData2ST (); + void setABCData2ST (); + void limit_cog (hrp::Vector3& cog); + bool vlimit(double& ret, const double llimit_value, const double ulimit_value); + void stopFootForEarlyTouchDown(); + void calcTouchoffRemainTime(); + void limbStretchAvoidanceControl(); + void calcTotalExternalForceZ(); + void fixActCogVelForWheel(); // for gg typedef boost::shared_ptr ggPtr; ggPtr gg; + typedef boost::shared_ptr stPtr; + stPtr st; bool gg_is_walking, gg_solved; // for abc - hrp::Vector3 ref_cog, ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset; + typedef boost::shared_ptr fikPtr; + fikPtr fik; + OpenHRP::AutoBalancerService::IKMode ik_mode; + hrp::Vector3 ref_cog, ref_zmp, rel_ref_zmp, prev_ref_zmp, prev_imu_sensor_pos, prev_imu_sensor_vel, hand_fix_initial_offset, orig_cog, prev_orig_cog, target_cog; enum {BIPED, TROT, PACE, CRAWL, GALLOP} gait_type; - enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode, return_control_mode; + enum {MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC} control_mode; std::map ikp; std::map contact_states_index_map; std::map m_vfs; std::vector sensor_names, leg_names, ee_vec; - hrp::dvector qorg, qrefv; - hrp::Vector3 current_root_p, target_root_p; - hrp::Matrix33 current_root_R, target_root_R; - rats::coordinates fix_leg_coords; + hrp::Vector3 target_root_p; + hrp::Matrix33 target_root_R; + rats::coordinates fix_leg_coords, fix_leg_coords2; std::vector default_zmp_offsets; - double m_dt, move_base_gain; + double m_dt; hrp::BodyPtr m_robot; coil::Mutex m_mutex; @@ -243,22 +357,59 @@ class AutoBalancer interpolator *transition_interpolator; interpolator *adjust_footstep_interpolator; interpolator *leg_names_interpolator; - hrp::Vector3 input_zmp, input_basePos; - hrp::Matrix33 input_baseRot; + interpolator *angular_momentum_interpolator; + interpolator *roll_weight_interpolator; + interpolator *pitch_weight_interpolator; + interpolator *roll_momentum_interpolator; + interpolator *pitch_momentum_interpolator; + interpolator *go_vel_interpolator; + interpolator *cog_constraint_interpolator; + interpolator *limit_cog_interpolator; + interpolator *hand_fix_interpolator; + hrp::Vector3 input_zmp, input_basePos, ref_basePos, baseRpy; + hrp::Matrix33 input_baseRot, ref_baseRot; // static balance point offsetting hrp::Vector3 sbp_offset, sbp_cog_offset; - enum {MODE_NO_FORCE, MODE_REF_FORCE} use_force; - std::vector ref_forces; + enum {MODE_NO_FORCE, MODE_REF_FORCE, MODE_REF_FORCE_WITH_FOOT, MODE_REF_FORCE_RFU_EXT_MOMENT, MODE_ADDITIONAL_FORCE} use_force; + std::vector ref_forces, ref_moments; unsigned int m_debugLevel; - bool is_legged_robot, is_stop_mode, has_ik_failed, is_hand_fix_mode, is_hand_fix_initial; - int loop, ik_error_debug_print_freq; + bool is_legged_robot, is_stop_mode, is_hand_fix_mode, is_hand_fix_initial; + int loop; bool graspless_manip_mode; std::string graspless_manip_arm; - hrp::Vector3 graspless_manip_p_gain; + hrp::Vector3 graspless_manip_p_gain, orig_dif_p, prev_orig_dif_p; rats::coordinates graspless_manip_reference_trans_coords; - double pos_ik_thre, rot_ik_thre; + + hrp::InvDynStateBuffer idsb; + std::vector invdyn_zmp_filters; + + // Used for ref force balancing. + hrp::Link* additional_force_applied_link; + hrp::Vector3 additional_force_applied_point_offset; + + bool is_after_walking; + hrp::Vector3 dif_ref_act_cog; + std::vector > support_polygon_vertices; + bool use_act_states; + std::vector diff_q; + interpolator *emergency_transition_interpolator; + interpolator *touch_wall_transition_interpolator; + hrp::Vector3 touchdown_foot_pos[2], prev_foot_pos[2]; + bool is_foot_touch[2]; + double touchoff_remain_time[2]; + std::map touchdown_transition_interpolator; + bool prev_roll_state, prev_pitch_state; + bool is_emergency_step_mode, is_emergency_touch_wall_mode, is_emergency_stopping, is_touch_wall_motion_solved, use_collision_avoidance, is_natural_walk, is_stop_early_foot; + double cog_z_constraint, touch_wall_retrieve_time, arm_swing_deg, total_external_force_z; + bool debug_read_steppable_region; + rats::coordinates initial_fix_coords; + bool was_exceed_q_ref_err_thre; + int ik_max_loop; + double d_wheel_angle, start_d_wheel_angle, wheel_radius; + std::map wheel_id; + std::map prev_wheel_pos; }; diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.cpp b/rtc/AutoBalancer/AutoBalancerService_impl.cpp index e4faf4ab27e..1a41a72b418 100644 --- a/rtc/AutoBalancer/AutoBalancerService_impl.cpp +++ b/rtc/AutoBalancer/AutoBalancerService_impl.cpp @@ -14,6 +14,16 @@ CORBA::Boolean AutoBalancerService_impl::goPos( CORBA::Double x, CORBA::Double return m_autobalancer->goPos(x, y, th); }; +CORBA::Boolean AutoBalancerService_impl::goPosWheel( CORBA::Double x, CORBA::Double y, CORBA::Double th, CORBA::Double w_x, CORBA::Double rv_max, CORBA::Double ra_max, CORBA::Double tm_off) +{ + return m_autobalancer->goPosWheel(x, y, th, w_x, rv_max, ra_max, tm_off); +}; + +CORBA::Boolean AutoBalancerService_impl::goWheel( CORBA::Double x, CORBA::Double rv_max, CORBA::Double ra_max) +{ + return m_autobalancer->goWheel(x, rv_max, ra_max); +}; + CORBA::Boolean AutoBalancerService_impl::goVelocity( CORBA::Double vx, CORBA::Double vy, CORBA::Double vth) { return m_autobalancer->goVelocity(vx, vy, vth); @@ -24,6 +34,11 @@ CORBA::Boolean AutoBalancerService_impl::goStop() return m_autobalancer->goStop(); }; +CORBA::Boolean AutoBalancerService_impl::jumpTo( CORBA::Double x, CORBA::Double y, CORBA::Double z, CORBA::Double ts, CORBA::Double tf) +{ + return m_autobalancer->jumpTo(x, y, z, ts, tf); +}; + CORBA::Boolean AutoBalancerService_impl::emergencyStop() { return m_autobalancer->emergencyStop(); @@ -39,6 +54,11 @@ CORBA::Boolean AutoBalancerService_impl::setFootStepsWithParam(const OpenHRP::Au return m_autobalancer->setFootStepsWithParam(fss, spss, overwrite_fs_idx); } +CORBA::Boolean AutoBalancerService_impl::setFootStepsWithWheel(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, const OpenHRP::AutoBalancerService::WheelParamsSequence& wpss, CORBA::Long overwrite_fs_idx) +{ + return m_autobalancer->setFootStepsWithWheel(fss, spss, wpss, overwrite_fs_idx); +} + void AutoBalancerService_impl::waitFootSteps() { return m_autobalancer->waitFootSteps(); @@ -59,6 +79,16 @@ CORBA::Boolean AutoBalancerService_impl::stopAutoBalancer() return m_autobalancer->stopAutoBalancer(); }; +void AutoBalancerService_impl::startStabilizer(void) +{ + m_autobalancer->startStabilizer(); +} + +void AutoBalancerService_impl::stopStabilizer(void) +{ + m_autobalancer->stopStabilizer(); +} + CORBA::Boolean AutoBalancerService_impl::setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param) { return m_autobalancer->setGaitGeneratorParam(i_param); @@ -67,7 +97,7 @@ CORBA::Boolean AutoBalancerService_impl::setGaitGeneratorParam(const OpenHRP::Au CORBA::Boolean AutoBalancerService_impl::getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam_out i_param) { i_param = new OpenHRP::AutoBalancerService::GaitGeneratorParam(); - i_param->stride_parameter.length(4); + i_param->stride_parameter.length(6); i_param->toe_heel_phase_ratio.length(7); i_param->zmp_weight_map.length(4); return m_autobalancer->getGaitGeneratorParam(*i_param); @@ -84,6 +114,17 @@ CORBA::Boolean AutoBalancerService_impl::getAutoBalancerParam(OpenHRP::AutoBalan return m_autobalancer->getAutoBalancerParam(*i_param); }; +void AutoBalancerService_impl::setStabilizerParam(const OpenHRP::AutoBalancerService::StabilizerParam& i_param) +{ + m_autobalancer->setStabilizerParam(i_param); +} + +void AutoBalancerService_impl::getStabilizerParam(OpenHRP::AutoBalancerService::StabilizerParam_out i_param) +{ + i_param = new OpenHRP::AutoBalancerService::StabilizerParam(); + return m_autobalancer->getStabilizerParam(*i_param); +} + CORBA::Boolean AutoBalancerService_impl::getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam_out i_param) { i_param = new OpenHRP::AutoBalancerService::FootstepParam(); diff --git a/rtc/AutoBalancer/AutoBalancerService_impl.h b/rtc/AutoBalancer/AutoBalancerService_impl.h index 3a4344084b2..a61eb7fe2d8 100644 --- a/rtc/AutoBalancer/AutoBalancerService_impl.h +++ b/rtc/AutoBalancer/AutoBalancerService_impl.h @@ -2,7 +2,8 @@ #ifndef AUTOBALANCERSERVICESVC_IMPL_H #define AUTOBALANCERSERVICESVC_IMPL_H -#include "AutoBalancerService.hh" +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/AutoBalancerService.hh" using namespace OpenHRP; @@ -16,19 +17,27 @@ class AutoBalancerService_impl AutoBalancerService_impl(); virtual ~AutoBalancerService_impl(); CORBA::Boolean goPos( CORBA::Double x, CORBA::Double y, CORBA::Double th); + CORBA::Boolean goPosWheel( CORBA::Double x, CORBA::Double y, CORBA::Double th, CORBA::Double w_x, CORBA::Double rv_max, CORBA::Double ra_max, CORBA::Double tm_off); + CORBA::Boolean goWheel( CORBA::Double x, CORBA::Double rv_max, CORBA::Double ra_max); CORBA::Boolean goVelocity( CORBA::Double vx, CORBA::Double vy, CORBA::Double vth); CORBA::Boolean goStop(); + CORBA::Boolean jumpTo( CORBA::Double x, CORBA::Double y, CORBA::Double z, CORBA::Double ts, CORBA::Double tf); CORBA::Boolean emergencyStop(); CORBA::Boolean setFootSteps(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, CORBA::Long overwrite_fs_idx); CORBA::Boolean setFootStepsWithParam(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, CORBA::Long overwrite_fs_idx); + CORBA::Boolean setFootStepsWithWheel(const OpenHRP::AutoBalancerService::FootstepsSequence& fss, const OpenHRP::AutoBalancerService::StepParamsSequence& spss, const OpenHRP::AutoBalancerService::WheelParamsSequence& wpss, CORBA::Long overwrite_fs_idx); void waitFootSteps(); void waitFootStepsEarly(CORBA::Double tm); CORBA::Boolean startAutoBalancer(const OpenHRP::AutoBalancerService::StrSequence& limbs); CORBA::Boolean stopAutoBalancer(); + void startStabilizer(void); + void stopStabilizer(void); CORBA::Boolean setGaitGeneratorParam(const OpenHRP::AutoBalancerService::GaitGeneratorParam& i_param); CORBA::Boolean getGaitGeneratorParam(OpenHRP::AutoBalancerService::GaitGeneratorParam_out i_param); CORBA::Boolean setAutoBalancerParam(const OpenHRP::AutoBalancerService::AutoBalancerParam& i_param); CORBA::Boolean getAutoBalancerParam(OpenHRP::AutoBalancerService::AutoBalancerParam_out i_param); + void setStabilizerParam(const OpenHRP::AutoBalancerService::StabilizerParam& i_param); + void getStabilizerParam(OpenHRP::AutoBalancerService::StabilizerParam_out i_param); CORBA::Boolean getFootstepParam(OpenHRP::AutoBalancerService::FootstepParam_out i_param); CORBA::Boolean adjustFootSteps(const OpenHRP::AutoBalancerService::Footstep& rfootstep, const OpenHRP::AutoBalancerService::Footstep& lfootstep); CORBA::Boolean getRemainingFootstepSequence(OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep , CORBA::Long& o_current_fs_idx); diff --git a/rtc/AutoBalancer/CMakeLists.txt b/rtc/AutoBalancer/CMakeLists.txt index 7f959e0f66d..b63320c3e74 100644 --- a/rtc/AutoBalancer/CMakeLists.txt +++ b/rtc/AutoBalancer/CMakeLists.txt @@ -1,5 +1,17 @@ -set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp) -set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +if(USE_QPOASES) + add_subdirectory(${CMAKE_CURRENT_BINARY_DIR}/../../3rdparty/qpOASES/qpOASES-3.0 ${CMAKE_CURRENT_BINARY_DIR}/../../3rdparty/qpOASES/qpOASES-3.0) + link_directories(${CMAKE_CURRENT_BINARY_DIR}/../../3rdparty/qpOASES/qpOASES-3.0/bin) + include_directories(${CMAKE_CURRENT_BINARY_DIR}/../../3rdparty/qpOASES/qpOASES-3.0/include) + add_definitions(-DUSE_QPOASES) +endif() + +set(comp_sources AutoBalancer.cpp AutoBalancerService_impl.cpp ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp PreviewController.cpp GaitGenerator.cpp SimpleFullbodyInverseKinematicsSolver.h ../TorqueFilter/IIRFilter.cpp Stabilizer.cpp FootGuidedController.cpp) +if(USE_QPOASES) + set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub qpOASES) +else() + set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +endif() + add_library(AutoBalancer SHARED ${comp_sources}) target_link_libraries(AutoBalancer ${libs}) set_target_properties(AutoBalancer PROPERTIES PREFIX "") @@ -7,8 +19,9 @@ set_target_properties(AutoBalancer PROPERTIES PREFIX "") add_executable(testPreviewController testPreviewController.cpp ../ImpedanceController/RatsMatrix.cpp PreviewController.cpp) target_link_libraries(testPreviewController ${libs}) -add_executable(testGaitGenerator testGaitGenerator.cpp ../ImpedanceController/RatsMatrix.cpp PreviewController.cpp GaitGenerator.cpp ../SequencePlayer/interpolator.cpp) +add_executable(testGaitGenerator testGaitGenerator.cpp ../ImpedanceController/RatsMatrix.cpp PreviewController.cpp GaitGenerator.cpp ../SequencePlayer/interpolator.cpp FootGuidedController.cpp) target_link_libraries(testGaitGenerator ${libs}) +set_target_properties(testGaitGenerator PROPERTIES COMPILE_DEFINITIONS "FOR_TESTGAITGENERATOR=1") add_executable(AutoBalancerComp AutoBalancerComp.cpp ${comp_sources}) target_link_libraries(AutoBalancerComp ${libs}) @@ -24,16 +37,22 @@ add_test(testGaitGeneratorTest2 testGaitGenerator --test2 --use-gnuplot false) add_test(testGaitGeneratorTest3 testGaitGenerator --test3 --use-gnuplot false) add_test(testGaitGeneratorTest4 testGaitGenerator --test4 --use-gnuplot false) add_test(testGaitGeneratorTest5 testGaitGenerator --test5 --use-gnuplot false) -#add_test(testGaitGeneratorTest6 testGaitGenerator --test6 --use-gnuplot false) +add_test(testGaitGeneratorTest6 testGaitGenerator --test6 --use-gnuplot false) add_test(testGaitGeneratorTest7 testGaitGenerator --test7 --use-gnuplot false) add_test(testGaitGeneratorTest8 testGaitGenerator --test8 --use-gnuplot false) add_test(testGaitGeneratorTest9 testGaitGenerator --test9 --use-gnuplot false) add_test(testGaitGeneratorTest10 testGaitGenerator --test10 --use-gnuplot false) add_test(testGaitGeneratorTest11 testGaitGenerator --test11 --use-gnuplot false) add_test(testGaitGeneratorTest12 testGaitGenerator --test12 --use-gnuplot false) +add_test(testGaitGeneratorTest13 testGaitGenerator --test13 --use-gnuplot false) +add_test(testGaitGeneratorTest14 testGaitGenerator --test14 --use-gnuplot false) +add_test(testGaitGeneratorTest15 testGaitGenerator --test15 --use-gnuplot false) +add_test(testGaitGeneratorTest16 testGaitGenerator --test16 --use-gnuplot false) +add_test(testGaitGeneratorTest17 testGaitGenerator --test17 --use-gnuplot false) +add_test(testGaitGeneratorTest18 testGaitGenerator --test18 --use-gnuplot false) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/AutoBalancer/FootGuidedController.cpp b/rtc/AutoBalancer/FootGuidedController.cpp new file mode 100644 index 00000000000..a1a52fbba88 --- /dev/null +++ b/rtc/AutoBalancer/FootGuidedController.cpp @@ -0,0 +1,120 @@ +/* -*- coding:utf-8-unix; mode:c++; -*- */ +#include "FootGuidedController.h" + + +void foot_guided_control_base::set_mat() +{ + xi = std::sqrt((g - force_z/mass) / dz); + h = 1 + xi * dt; + h_ = 1 - xi * dt; + A << + 1.0, dt, + xi * xi * dt, 1.0; + b << + 0.0, + -xi * xi * dt; + Phi << + 1.0, 1.0 / xi, + 1.0, -1.0 / xi; + Phi_inv << + 1.0, 1.0, + xi, -xi; + Phi_inv = 2.0 * Phi_inv; + g_k << + 0.0, + -(g - force_z/mass) * dt; +} + +double foot_guided_control_base::calc_u(const std::vector >& ref_zmp, const double cur_cp, const bool is_z) +{ + double u = cur_cp - ref_zmp.front().getStart() - ref_zmp.front().getSlope() / xi; // 数式上の j = 0 に相当 + double Tj = 0.0; + for (size_t j = 0; j < ref_zmp.size() - 1; j++) { // 数式上のindexとは1ずれてる + Tj += ref_zmp.at(j).getTime(); + u += exp(- xi * Tj) * (ref_zmp.at(j).getGoal() - ref_zmp.at(j+1).getStart() + (ref_zmp.at(j).getSlope() - ref_zmp.at(j+1).getSlope()) / xi); + } + if (is_z) u -= dz; + u = ref_zmp.front().getStart() + 2 * u / (1 - exp(-2 * xi * Tj)); + + return u; +} + +void foot_guided_control_base::truncate_u() +{ + double acc; + get_acc(acc); + if (std::abs(acc) > mu * g) { + acc = mu * g * sgn(acc); + u_k = x_k(0) - acc / (xi * xi); + } +} + +// assumed after calc_u +void foot_guided_control_base::calc_x_k(const bool is_z) +{ + if (1) { // reference version appears to be more robust + // if (is_z) { // follow reference for height + x_k = A * x_k + b * u_k; + } else { // sec.6.1 in "Foot-guided control of a biped robot through ZMP manipulation" + double coef = 2.5; + double diff_p = (act_x_k(0) - x_k(0)); + double diff_v = (act_x_k(1) - x_k(1)); + double acc = xi * xi * (act_x_k(0) - act_u_k); + x_k(1) = x_k(1) + (acc + coef * diff_v) * dt; + x_k(0) = x_k(0) + (x_k(1) + coef * diff_p) * dt; + } + if (is_z) x_k += g_k; +} + +void foot_guided_control_base::update_control(double& zmp, double& feedforward_zmp, const std::vector >& ref_zmp, const bool is_z) +{ + u_k = calc_u(ref_zmp, (Phi * x_k)(0), is_z); + act_u_k = calc_u(ref_zmp, (Phi * act_x_k)(0), is_z); + + feedforward_zmp = u_k; + zmp = act_u_k; +} + +void foot_guided_control_base::update_state(double& pos, const double fx, const bool is_z) +{ + calc_x_k(is_z); + dc_off = - fx / (xi * xi * mass); // 反力 + pos = x_k(0) + dc_off; +} + +double foot_guided_control_base::calc_u_jump(const double landing_pos, const double takeoff_height, const double flight_time, const double cur_cog, const double cur_cogvel, const double ref_zmp, const double remain_time, const bool is_z) +{ + double u; + if (is_z) { // vertical control + const double takeoff_z_vel = (landing_pos - takeoff_height) / flight_time + g * flight_time / 2.0; + const double z_d = takeoff_height - cur_cog + (std::exp(-xi * remain_time) * takeoff_z_vel - cur_cogvel) / xi; + const double z_c = takeoff_height - cur_cog - (std::exp(xi * remain_time) * takeoff_z_vel - cur_cogvel) / xi; + const double t_d = (std::exp(2 * xi * remain_time) - 1) / (2 * xi); + const double t_c = (std::exp(-2 * xi * remain_time) - 1) / (2 * xi); + u = ref_zmp - ((remain_time - t_d) * z_d - (remain_time + t_c) * z_c) / (xi * (remain_time * remain_time + t_c * t_d)); + } else { // horizontal control + const double r_f = xi * flight_time / 2.0; + const double x_dp = cur_cog + cur_cogvel / xi - ref_zmp; + const double x_cp = cur_cog - cur_cogvel / xi - ref_zmp; + const double x_sp = landing_pos - ref_zmp; + u = ref_zmp + -2 * (x_sp - std::exp(xi * remain_time) * x_dp - r_f * (std::exp(xi * remain_time) * x_dp - std::exp(-xi * remain_time) * x_cp)) + / (std::exp(xi * remain_time) - std::exp(-xi * remain_time) + r_f * (std::exp(xi * remain_time) - std::exp(-xi * remain_time) + 2 * xi * std::exp(-xi * remain_time) * remain_time)); + } + return u; +} + +void foot_guided_control_base::update_control_jump(double& zmp, double& feedforward_zmp, const double ref_zmp, const double landing_pos, const double takeoff_height, const double flight_time, const double remain_time, const bool is_z) +{ + u_k = calc_u_jump(landing_pos, takeoff_height, flight_time, x_k(0), x_k(1), ref_zmp, remain_time, is_z); + act_u_k = calc_u_jump(landing_pos, takeoff_height, flight_time, act_x_k(0), act_x_k(1), ref_zmp, remain_time, is_z); + + feedforward_zmp = u_k; + zmp = act_u_k; +} + +// void foot_guided_control_base::update(double& zmp, double& pos, const std::size_t N, const double ref_dcm, const double ref_zmp) +// { +// update_control(zmp, N, ref_dcm, ref_zmp); +// update_state(pos); +// } diff --git a/rtc/AutoBalancer/FootGuidedController.h b/rtc/AutoBalancer/FootGuidedController.h new file mode 100644 index 00000000000..7a50d403875 --- /dev/null +++ b/rtc/AutoBalancer/FootGuidedController.h @@ -0,0 +1,240 @@ +/* -*- coding:utf-8-unix mode:c++ -*- */ +#ifndef FOOT_H_ +#define FOOT_H_ +#include +#include +#include +#include "hrpsys/util/Hrpsys.h" +#include +#include "../TorqueFilter/IIRFilter.h" + +static const double DEFAULT_GRAVITATIONAL_ACCELERATION = 9.80665; // [m/s^2] + +template int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +template +class LinearTrajectory +{ +private: + T a, b, start, goal; + double time, time_off; +public: + LinearTrajectory(const T& _start, const T& _goal, const double _t, const double _t_off = 0.0) + : start(_start), goal(_goal), time_off(_t_off) { + time = std::max(_t, 2e-3); + a = (_goal - _start) / time; + b = _start - a * _t_off; + }; + const T& getSlope() const { return a; }; + const T& getIntercept() const { return b; }; + const T& getStart() const { return start; }; + const T& getGoal() const { return goal; }; + T getPosFromTime(const double _t) const { return a * _t + b; }; + T getPosFromRatio(const double ratio) const { return a * time * ratio + b; }; + double getTime() const { return time; }; + double getTimeOff() const { return time_off; }; + + // friend const LinearTrajectory& operator+(const LinearTrajectory& l, const LinearTrajectory& r) + LinearTrajectory operator+(const LinearTrajectory& r) const + { + const double t0 = std::max(this->getTimeOff(), r.getTimeOff()); + const double t1 = std::min(this->getTimeOff() + this->getTime(), r.getTimeOff() + r.getTime()); + const T s = this->getPosFromTime(t0) + r.getPosFromTime(t0); + const T g = this->getPosFromTime(t1) + r.getPosFromTime(t1); + return LinearTrajectory(s, g, t1 - t0); + }; +}; + +class foot_guided_control_base +{ +private: + double calc_u(const std::vector >& ref_zmp, const double cur_cp, const bool is_z = false); + double calc_u_jump(const double landing_pos, const double takeoff_height, const double flight_time, const double cur_cog, const double cur_cogvel, const double ref_zmp, const double remain_time, const bool is_z = false); + void truncate_u(); + void calc_x_k(const bool is_z = false); +protected: + Eigen::Matrix A; // state matrix + Eigen::Matrix b; // input matrix + Eigen::Matrix Phi; // convert matrix : pos, vel => dcm, ccm + Eigen::Matrix Phi_inv; // convert matrix : dcm, ccm => pos, vel + Eigen::Matrix x_k; // pos, vel + Eigen::Matrix act_x_k; // pos, vel + Eigen::Matrix w_k; // dcm, ccm + Eigen::Matrix act_w_k; // dcm, ccm + Eigen::Matrix g_k; + double u_k; // zmp + double act_u_k; // zmp + double w_k_offset; // dcm offset + double mu; // friction coefficient + double dt, g; + double dz, xi, h, h_; + double act_vel_ratio; // how much act_vel is used (0.0 ~ 1.0) + double dc_off; // offset for disturbance compensation + double mass; + double force_z; + boost::shared_ptr > zmp_filter; +public: + // constructor + foot_guided_control_base() {} + foot_guided_control_base(const double _dt, const double _dz, const double _mass, const double _freq, const double _force = 0.0, + const double _g = DEFAULT_GRAVITATIONAL_ACCELERATION) + : dt(_dt), dz(_dz), g(_g), mass(_mass), act_vel_ratio(1.0), force_z(_force), + x_k(Eigen::Matrix::Zero()), act_x_k(Eigen::Matrix::Zero()), u_k(0.0), act_u_k(0.0), w_k_offset(0.0), mu(0.5) + { + set_mat(); + zmp_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(_freq, _dt, 0.0)); + } + foot_guided_control_base(const double _dt, const double _dz, const double init_xk, const double _mass, const double _freq, const double _force = 0.0, + const double _g = DEFAULT_GRAVITATIONAL_ACCELERATION) + : dt(_dt), dz(_dz), g(_g), mass(_mass), act_vel_ratio(1.0), force_z(_force), + x_k(Eigen::Matrix::Zero()), act_x_k(Eigen::Matrix::Zero()), u_k(0.0), act_u_k(0.0), w_k_offset(0.0), mu(0.5) + { + set_mat(); + act_x_k(0) = x_k(0) = init_xk; + zmp_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(_freq, _dt, 0.0)); + } + // destructor + ~foot_guided_control_base() {}; + // update function + void update_control(double& zmp, double& feedforward_zmp, const std::vector >& ref_zmp, const bool is_z = false); + void update_control_jump(double& zmp, double& feedforward_zmp, const double ref_zmp, const double landing_pos, const double takeoff_height, const double flight_time, const double remain_time, const bool is_z = false); + void update_state(double& pos, const double fx, const bool is_z = false); + void update(double& zmp, double& pos, const std::size_t N, const double ref_dcm, const double ref_zmp); + // set function + void set_mat(); + void set_act_x_k(const double pos, const double vel, const bool is_start_or_end_phase) + { + act_x_k(0) = pos; + act_x_k(1) = vel; + } + void set_x_k(const double pos, const double vel) + { + x_k(0) = pos; + x_k(1) = vel; + } + void set_pos (const double x) { x_k(0) = x; } + void set_dc_off (const double off) { dc_off = off; } + void set_zmp (const double act_u, const double u) { + act_u_k = act_u; + u_k = u; + } + void set_offset (const double offset) { w_k_offset = offset; } + void set_dz(const double _dz, const double _force) { + dz = _dz; + force_z = _force; + set_mat(); + }; + void set_act_vel_ratio (const double ratio) { act_vel_ratio = ratio; } + // get_function + void get_pos (double& ret) { ret = x_k(0); } + void get_dc_off (double& ret) { ret = dc_off; } + void get_vel (double& ret) { ret = x_k(1); } + void get_acc (double& ret) { ret = xi * xi * ( x_k(0) - u_k ); } + void get_zmp (double& ret) { ret = u_k; } + double get_xi() { return xi; } + double get_dz() { return dz; } +}; + +template +class foot_guided_controller +{ +private: + foot_guided_control_base *controllers; +protected: +public: + // constructor + foot_guided_controller(const double _dt, const double _dz, const hrp::Vector3& init_xk, const double _mass, const double _freq, const double _force = 0.0, + const double _g = DEFAULT_GRAVITATIONAL_ACCELERATION) + { + controllers = new foot_guided_control_base[dim]; + for (size_t i = 0; i < dim; i++) controllers[i] = foot_guided_control_base(_dt, _dz, init_xk[i], _mass, _freq, _force, _g); + } + // destructor + ~foot_guided_controller() + { + delete[] controllers; + }; + // update function + void update_control(hrp::Vector3& zmp, hrp::Vector3& feedforward_zmp, const std::vector >& ref_zmp) + { + for (size_t i = 0; i < dim; i++) { + std::vector > rz; // TODO: 無駄な処理をなくす + rz.reserve(ref_zmp.size()); + for (size_t j = 0; j < ref_zmp.size(); j++) rz.push_back(LinearTrajectory(ref_zmp.at(j).getStart()(i), ref_zmp.at(j).getGoal()(i), ref_zmp.at(j).getTime())); + controllers[i].update_control(zmp[i], feedforward_zmp[i], rz, (i == 2)); + } + } + void update_control_jump(hrp::Vector3& zmp, hrp::Vector3& feedforward_zmp, const hrp::Vector3 ref_zmp, const hrp::Vector3 landing_pos, const double takeoff_height, const double flight_time, const double remain_time) + { + for (size_t i = 0; i < dim; i++) { + controllers[i].update_control_jump(zmp[i], feedforward_zmp[i], ref_zmp[i], landing_pos[i], takeoff_height, flight_time, remain_time, (i == 2)); + } + } + void update_state(hrp::Vector3& x_ret, const hrp::Vector3 fx) + { + for (size_t i = 0; i < dim; i++) + controllers[i].update_state(x_ret[i], fx[i], (i == 2)); + } + void update(hrp::Vector3& p_ret, hrp::Vector3& x_ret, const std::size_t N, const hrp::Vector3& ref_dcm, const hrp::Vector3& ref_zmp) + { + for (size_t i = 0; i < dim; i++) + controllers[i].update(p_ret[i], x_ret[i], N, ref_dcm[i], ref_zmp[i]); + } + // set function + void set_dc_off(const hrp::Vector3& off) + { + for (size_t i = 0; i < dim; i++) + controllers[i].set_dc_off(off[i]); + } + void set_offset(const hrp::Vector3& offset) + { + for (size_t i = 0; i < dim; i++) + controllers[i].set_offset(offset[i]); + } + void set_act_x_k (const hrp::Vector3& pos, const hrp::Vector3& vel, const bool is_start_or_end_phase) + { + for (size_t i = 0; i < dim; i++) + controllers[i].set_act_x_k(pos(i), vel(i), is_start_or_end_phase); + } + void set_x_k (const hrp::Vector3& pos, const hrp::Vector3& vel) + { + for (size_t i = 0; i < dim; i++) + controllers[i].set_x_k(pos(i), vel(i)); + } + void set_act_vel_ratio (const double ratio) { + for (size_t i = 0; i < dim; i++) { + controllers[i].set_act_vel_ratio(ratio); + } + } + void set_zmp (const hrp::Vector3& zmp, const hrp::Vector3& fzmp) { + for (size_t i = 0; i < dim; i++) { + controllers[i].set_zmp(zmp(i), fzmp(i)); + } + } + // get function + double get_dz() { return controllers[0].get_dz(); } + void get_pos(hrp::Vector3& ret) { + for (size_t i = 0; i < dim; i++) + controllers[i].get_pos(ret[i]); + } + void get_dc_off(hrp::Vector3& ret) { + for (size_t i = 0; i < dim; i++) + controllers[i].get_dc_off(ret[i]); + } + void get_vel(hrp::Vector3& ret) { + for (size_t i = 0; i < dim; i++) + controllers[i].get_vel(ret[i]); + } + void get_acc(hrp::Vector3& ret) { + for (size_t i = 0; i < dim; i++) + controllers[i].get_acc(ret[i]); + } + void set_dz(const double _dz, const double _force) { + for (size_t i = 0; i < dim; i++) { + controllers[i].set_dz(_dz, _force); + } + } +}; +#endif /*FOOT_H_*/ diff --git a/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h new file mode 100644 index 00000000000..ac26145336e --- /dev/null +++ b/rtc/AutoBalancer/FullbodyInverseKinematicsSolver.h @@ -0,0 +1,477 @@ +#ifndef FULLBODYIK_H +#define FULLBODYIK_H + +#include +#include +#include +#include "../ImpedanceController/JointPathEx.h" +#include +#include "SimpleFullbodyInverseKinematicsSolver.h" +#include "../TorqueFilter/IIRFilter.h" + +//tips +#define LIMIT_MINMAX(x,min,max) ((x= (xnumJoints()); for(int i=0;i<_robot->numJoints();i++){ tmp(i) = _robot->joint(i)->q; } return tmp; }; + static void setQAll(hrp::BodyPtr _robot, const hrp::dvector in){ assert(in.size() <= _robot->numJoints()); for(int i=0;i<_robot->numJoints();i++){ _robot->joint(i)->q = in(i); } }; +} + + +class IKConstraint { + public: + std::string target_link_name; + hrp::Vector3 targetPos; + hrp::Vector3 targetRpy; + hrp::Vector3 localPos; + hrp::Matrix33 localR; + hrp::dvector6 constraint_weight; + double pos_precision, rot_precision; + IKConstraint () + :targetPos(hrp::Vector3::Zero()), + targetRpy(hrp::Vector3::Zero()), + localPos(hrp::Vector3::Zero()), + localR(hrp::Matrix33::Identity()), + constraint_weight(hrp::dvector6::Ones()), + pos_precision(1e-4), rot_precision(deg2rad(0.1)){} +}; + +class CollisionData +{ + public: + int base_id, target_id; + hrp::Vector3 base_localp, target_localp, dir; + double safe_d1, safe_d0, cur_d, safe_d_offset; + bool is_collision; + CollisionData (const int _base_id, const hrp::Vector3& _base_localp, const int _target_id, const hrp::Vector3& _target_local_p, const double _safe_d1 = 0.2, const double _safe_d0 = 0.25) + : base_id(_base_id), target_id(_target_id), base_localp(_base_localp), target_localp(_target_local_p), dir(hrp::Vector3::Zero()), cur_d(0.0), is_collision(false), + safe_d_offset(1e-3), safe_d1(_safe_d1), safe_d0(_safe_d0) {}; + void reset () { + is_collision = false; + } + void checkCollision (const hrp::BodyPtr _robot) { + if (base_id != target_id) { + hrp::Vector3 base_pos = _robot->joint(base_id)->p + _robot->joint(base_id)->R * base_localp; + hrp::Vector3 target_pos = _robot->joint(target_id)->p + _robot->joint(target_id)->R * target_localp; + dir = (target_pos - base_pos).normalized(); + cur_d = (target_pos - base_pos).norm(); + if (cur_d < safe_d0) is_collision = true; + else is_collision = false; + } + } + double calcWeight (const double e) { + double a = std::log(e) / (safe_d0 - safe_d1); + return std::min(std::exp(a * (cur_d - safe_d1)), 1.0); + } +}; + +class FullbodyInverseKinematicsSolver : public SimpleFullbodyInverseKinematicsSolver{ + protected: + hrp::dmatrix J_all, J_all_inv, J_com, J_am; + hrp::dvector dq_all, err_all, constraint_weight_all, jlim_avoid_weight_old; + const int WS_DOF, BASE_DOF, J_DOF, ALL_DOF; + std::vector am_filters; + public: + hrp::dvector dq_weight_all, q_ref, q_ref_max_dq, q_ref_constraint_weight; + double q_ref_err_thre; + bool is_exceed_q_ref_err_thre; + hrp::Vector3 cur_momentum_around_COM, cur_momentum_around_COM_filtered, rootlink_rpy_llimit, rootlink_rpy_ulimit; + std::vector arm_collision; + FullbodyInverseKinematicsSolver (hrp::BodyPtr _robot, const std::string& _print_str, const double _dt) + : SimpleFullbodyInverseKinematicsSolver(_robot,_print_str, _dt), + WS_DOF(6), + BASE_DOF(6), + J_DOF(_robot->numJoints()), + ALL_DOF(J_DOF+BASE_DOF){ + dq_weight_all = hrp::dvector::Ones(ALL_DOF); + dq_all = q_ref = q_ref_constraint_weight = hrp::dvector::Zero(ALL_DOF); + q_ref_max_dq = hrp::dvector::Constant(ALL_DOF, 0.00174532); // 0.1deg + cur_momentum_around_COM = cur_momentum_around_COM_filtered = hrp::Vector3::Zero(); +// jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 1e12); + jlim_avoid_weight_old = hrp::dvector::Constant(ALL_DOF, 0); + rootlink_rpy_llimit = hrp::Vector3::Constant(-DBL_MAX); + rootlink_rpy_ulimit = hrp::Vector3::Constant(DBL_MAX); + am_filters.resize(3); + for(int i=0;i<3;i++){ + am_filters[i].setParameterAsBiquad(10, 1/std::sqrt(2), 1.0/m_dt); + am_filters[i].reset(); + } + arm_collision.reserve(4); + // RHP + if (_robot->link("R_CROTCH_R") != NULL && _robot->link("R_WRIST_Y") != NULL) { + arm_collision.push_back(CollisionData(_robot->link("R_CROTCH_R")->jointId, + hrp::Vector3(0.02, 0, -0.2), + _robot->link("R_WRIST_Y")->jointId, + hrp::Vector3(0, 0, -0.2))); + } + if (_robot->link("L_CROTCH_R") != NULL && _robot->link("L_WRIST_Y") != NULL) { + arm_collision.push_back(CollisionData(_robot->link("L_CROTCH_R")->jointId, + hrp::Vector3(0.02, 0, -0.2), + _robot->link("L_WRIST_Y")->jointId, + hrp::Vector3(0, 0, -0.2))); + } + // JAXON + if (_robot->link("RLEG_JOINT2") != NULL && _robot->link("RARM_JOINT6") != NULL) { + arm_collision.push_back(CollisionData(_robot->link("RLEG_JOINT2")->jointId, + hrp::Vector3(0, 0, -0.2), + _robot->link("RARM_JOINT6")->jointId, + hrp::Vector3(0, 0, -0.2))); + } + if (_robot->link("LLEG_JOINT2") != NULL && _robot->link("LARM_JOINT6") != NULL) { + arm_collision.push_back(CollisionData(_robot->link("LLEG_JOINT2")->jointId, + hrp::Vector3(0, 0, -0.2), + _robot->link("LARM_JOINT6")->jointId, + hrp::Vector3(0, 0, -0.2))); + } + if (_robot->link("CHEST_JOINT2") != NULL && _robot->link("RARM_JOINT3") != NULL) { + arm_collision.push_back(CollisionData(_robot->link("CHEST_JOINT2")->jointId, + hrp::Vector3(0, 0, 0), + _robot->link("RARM_JOINT3")->jointId, + hrp::Vector3(0, 0, 0), + 0.32, 0.33)); + } + if (_robot->link("CHEST_JOINT2") != NULL && _robot->link("LARM_JOINT3") != NULL) { + arm_collision.push_back(CollisionData(_robot->link("CHEST_JOINT2")->jointId, + hrp::Vector3(0, 0, 0), + _robot->link("LARM_JOINT3")->jointId, + hrp::Vector3(0, 0, 0), + 0.32, 0.33)); + } + is_exceed_q_ref_err_thre = false; + q_ref_err_thre = deg2rad(30.0); + }; + ~FullbodyInverseKinematicsSolver () {}; + bool checkIKConvergence(const hrp::BodyPtr _robot, const std::vector& _ikc_list){ + for ( int i=0; i<_ikc_list.size(); i++ ) { + hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name); + hrp::Vector3 pos_err, rot_err; + if(link_tgt_ptr){ + pos_err = _ikc_list[i].targetPos - (link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos); + rats::difference_rotation(rot_err, (link_tgt_ptr->R * _ikc_list[i].localR), hrp::rotFromRpy(_ikc_list[i].targetRpy)); + } + else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ // COM + pos_err = _ikc_list[i].targetPos - (_robot->calcCM() + _ikc_list[i].localR * _ikc_list[i].localPos); + rot_err = _ikc_list[i].targetRpy - cur_momentum_around_COM; + } + else{ std::cerr<<"Unknown Link Target !!"< 0 && fabs(pos_err(j)) > _ikc_list[i].pos_precision){return false;} + if(_ikc_list[i].constraint_weight.tail(3)(j) > 0 && fabs(rot_err(j)) > _ikc_list[i].rot_precision){return false;} + } + } + return true; + } + int solveFullbodyIKLoop (hrp::BodyPtr _robot, const std::vector& _ikc_list, const int _max_iteration) { + #ifdef OPENHRP_PACKAGE_VERSION_320 + hrp::Vector3 base_p_old = _robot->rootLink()->p; + hrp::Matrix33 base_R_old = _robot->rootLink()->R; + hrp::dvector q_old(J_DOF); + for(int i=0;ijoint(i)->q; } + unsigned int loop; + for(loop=0; loop < _max_iteration;){ + solveFullbodyIKOnce(_robot, _ikc_list); + //check ang moment + _robot->rootLink()->v = (_robot->rootLink()->p - base_p_old)/ m_dt; + _robot->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot->rootLink()->R) / m_dt; + for(int i=0;ijoint(i)->dq = (_robot->joint(i)->q - q_old(i)) / m_dt; } + _robot->calcForwardKinematics(true,false); + hrp::Vector3 tmp_P, tmp_L; + _robot->calcTotalMomentum(tmp_P, tmp_L);//calcTotalMomentumは漸化的にWorld周りの並進＋回転運動量を出す + cur_momentum_around_COM = tmp_L - _robot->calcCM().cross(tmp_P); +// _robot->calcTotalMomentumFromJacobian(tmp_P, cur_momentum_around_COM);//calcTotalMomentumFromJacobianは重心ヤコビアンと重心周り角運動量ヤコビアンを用いて重心周りの並進＋回転運動量を出す + // few iteration cause vaibration of momentum + if (_max_iteration > 2) { + cur_momentum_around_COM_filtered = cur_momentum_around_COM; + } else { + for(int i=0; i<3; i++){ + cur_momentum_around_COM_filtered(i) = am_filters[i].passFilter(cur_momentum_around_COM(i)); + } + } + loop++; + if(checkIKConvergence(_robot, _ikc_list)){ break; } + } + return loop; + #else + std::cerr<<"solveFullbodyIKLoop() needs OPENHRP_PACKAGE_VERSION_320 !!!"<& _ikc_list){ + #ifdef OPENHRP_PACKAGE_VERSION_320 + // count all constraint DOF and allocate Jacobian and vectors + int C_DOF = 0; // constraint DOF + for (int i=0;i<_ikc_list.size();i++){ C_DOF += (_ikc_list[i].constraint_weight.array()>0.0).count(); } + C_DOF += (q_ref_constraint_weight.array()>0.0).count(); + J_all = hrp::dmatrix::Zero(C_DOF, ALL_DOF); + err_all = hrp::dvector::Zero(C_DOF); + constraint_weight_all = hrp::dvector::Ones(C_DOF); + + // set end effector constraint into Jacobian + int cur_cid_all = 0; //current constraint index of all constraints + for ( int i=0; i<_ikc_list.size(); i++ ) { + hrp::Link* link_tgt_ptr = _robot->link(_ikc_list[i].target_link_name); + hrp::dmatrix J_part = hrp::dmatrix::Zero(WS_DOF, ALL_DOF); //全身to拘束点へのヤコビアン + hrp::dvector6 dp_part; //pos + rot 速度ではなく差分 + if(link_tgt_ptr){//ベースリンク，通常リンク共通 + hrp::Vector3 tgt_cur_pos = link_tgt_ptr->p + link_tgt_ptr->R * _ikc_list[i].localPos; + hrp::Matrix33 tgt_cur_rot = link_tgt_ptr->R * _ikc_list[i].localR; + dp_part.head(3) = _ikc_list[i].targetPos - tgt_cur_pos; + dp_part.tail(3) = tgt_cur_rot * omegaFromRotEx(tgt_cur_rot.transpose() * hrp::rotFromRpy(_ikc_list[i].targetRpy)); + hrp::JointPathEx tgt_jpath(_robot, _robot->rootLink(), link_tgt_ptr, m_dt, false, ""); + hrp::dmatrix J_jpath(6, tgt_jpath.numJoints()); + tgt_jpath.calcJacobian(J_jpath, _ikc_list[i].localPos); + for(int id_in_jpath=0; id_in_jpathjointId) = J_jpath.col(id_in_jpath); } //ジョイントパスのJacobianを全身用に並び替え + J_part.rightCols(BASE_DOF) = hrp::dmatrix::Identity( WS_DOF, BASE_DOF ); + J_part.rightCols(BASE_DOF).topRightCorner(3,3) = - hrp::hat(tgt_cur_pos - _robot->rootLink()->p); + }else if(!link_tgt_ptr && _ikc_list[i].target_link_name == "COM"){ //重心限定 + dp_part.head(3) = _ikc_list[i].targetPos - _robot->calcCM(); +// dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける + dp_part.tail(3) = (_ikc_list[i].targetRpy - cur_momentum_around_COM_filtered) * m_dt;// COMのrotはAngulerMomentumとして扱う&差分なのでdtかける(+フィルタ) + _robot->calcCMJacobian(NULL, J_com);//デフォで右端に3x6のbase->COMのヤコビアンが付いてくる + _robot->calcAngularMomentumJacobian(NULL, J_am);//world座標系での角運動量を生成するヤコビアン(なので本当はCOM周りには使えない) + J_part << J_com, J_am; + }else{ std::cerr<<"Unknown Link Target !!"< 0.0){ + J_all.row(cur_cid_all) = J_part.row(cid); + err_all(cur_cid_all) = dp_part(cid); + constraint_weight_all(cur_cid_all) = _ikc_list[i].constraint_weight(cid); + cur_cid_all++; + } + } + } + + // set desired joint pose constraint into Jacobian + double q_err = 0.0; + for(int qid=0; qid 0.0){ + J_all.row(cur_cid_all)(qid) = 1; + err_all(cur_cid_all) = (qid < J_DOF) ? (q_ref(qid) - _robot->joint(qid)->q) : (q_ref(qid) - hrp::rpyFromRot(_robot->rootLink()->R)(qid-J_DOF)); + q_err += err_all(cur_cid_all) * err_all(cur_cid_all); + LIMIT_MINMAX(err_all(cur_cid_all), -q_ref_max_dq(qid), q_ref_max_dq(qid)); + constraint_weight_all(cur_cid_all) = q_ref_constraint_weight(qid); + cur_cid_all++; + } + } + is_exceed_q_ref_err_thre = (sqrt(q_err) > q_ref_err_thre); + + // joint limit avoidance (copy from JointPathEx) + hrp::dvector dq_weight_all_jlim = hrp::dvector::Ones(ALL_DOF); + for ( int j = 0; j < ALL_DOF ; j++ ) { + double jang, jmax, jmin; + if(jjoint(j)->q; + jmax = m_robot->joint(j)->ulimit; + jmin = m_robot->joint(j)->llimit; + }else if(jrootLink()->R)(rpy_id); + jmax = rootlink_rpy_ulimit(rpy_id); + jmin = rootlink_rpy_llimit(rpy_id); + } + const double e = deg2rad(1); + if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) { + } else if ( eps_eq(jang, jmax,e) ) { + jang = jmax - e; + } else if ( eps_eq(jang, jmin,e) ) { + jang = jmin + e; + } + double jlim_avoid_weight; + if ( eps_eq(jang, jmax,e) && eps_eq(jang, jmin,e) ) { + jlim_avoid_weight = DBL_MAX; + } else { + jlim_avoid_weight = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) ); + if (std::isnan(jlim_avoid_weight)) jlim_avoid_weight = 0; + } + if (( jlim_avoid_weight - jlim_avoid_weight_old(j) ) >= 0 ) { // add weight only if q approaching to the limit + dq_weight_all_jlim(j) += jlim_avoid_weight; + } + jlim_avoid_weight_old(j) = jlim_avoid_weight; + } + hrp::dvector dq_weight_all_final = dq_weight_all.array() * dq_weight_all_jlim.array(); + + // Solvability-unconcerned Inverse Kinematics by Levenberg-Marquardt Method [sugihara:RSJ2009] + const double wn_const = 1e-6; + hrp::dmatrix Wn = (static_cast(err_all.transpose() * constraint_weight_all.asDiagonal() * err_all) + wn_const) * hrp::dmatrix::Identity(ALL_DOF, ALL_DOF); + Wn = dq_weight_all_final.asDiagonal() * Wn; + hrp::dmatrix H = J_all.transpose() * constraint_weight_all.asDiagonal() * J_all + Wn; + hrp::dvector g = J_all.transpose() * constraint_weight_all.asDiagonal() * err_all; + // dq_all = H.inverse() * g; + dq_all = H.ldlt().solve(g); + + // // debug print + // static int count; + // if(count++ % 10000 == 0){ + // std::cout<numJoints();i++)std::cerr<<_robot->joint(i)->q<<" "; + // std::cout<joint(i)->q += dq_all(i); + LIMIT_MINMAX(_robot->joint(i)->q, _robot->joint(i)->llimit, _robot->joint(i)->ulimit); + } + + // update rootlink pos rot + _robot->rootLink()->p += dq_all.tail(6).head(3); + for(int i=0;i<3;i++){ + if(hrp::rpyFromRot(_robot->rootLink()->R)(i) < rootlink_rpy_llimit(i) && dq_all.tail(6).tail(3)(i) < 0) dq_all.tail(6).tail(3)(i) = 0; + if(hrp::rpyFromRot(_robot->rootLink()->R)(i) > rootlink_rpy_ulimit(i) && dq_all.tail(6).tail(3)(i) > 0) dq_all.tail(6).tail(3)(i) = 0; + } + + + hrp::Matrix33 dR; + hrp::Vector3 omega = dq_all.tail(6).tail(3); + hrp::calcRodrigues(dR, omega.normalized(), omega.norm()); + rats::rotm3times(_robot->rootLink()->R, dR, _robot->rootLink()->R); + _robot->calcForwardKinematics(); + + // Collision check + for (size_t i = 0; i < arm_collision.size(); i++) { + arm_collision[i].checkCollision(_robot); + } + #else + std::cerr<<"solveFullbodyIKOnce() needs OPENHRP_PACKAGE_VERSION_320 !!!"<rootLink()->p = current_root_p; + m_robot->rootLink()->R = current_root_R; + m_robot->calcForwardKinematics(); + }; + void resetCollision () { + for(size_t i = 0; i < arm_collision.size(); i++) { + arm_collision[i].reset(); + } + } + + protected: + hrp::Vector3 omegaFromRotEx(const hrp::Matrix33& r) {//copy from JointPathEx.cpp + using ::std::numeric_limits; + double alpha = (r(0,0) + r(1,1) + r(2,2) - 1.0) / 2.0; + if(fabs(alpha - 1.0) < 1.0e-12) { //th=0,2PI; + return hrp::Vector3::Zero(); + } else { + double th = acos(alpha); + double s = sin(th); + if (s < numeric_limits::epsilon()) { //th=PI + return hrp::Vector3( sqrt((r(0,0)+1)*0.5)*th, sqrt((r(1,1)+1)*0.5)*th, sqrt((r(2,2)+1)*0.5)*th ); + } + double k = -0.5 * th / s; + return hrp::Vector3( (r(1,2) - r(2,1)) * k, (r(2,0) - r(0,2)) * k, (r(0,1) - r(1,0)) * k ); + } + } +}; + + +// // multi-thread IK (answer will delay for 1 control loop) +//class FullbodyInverseKinematicsSolverMT : public FullbodyInverseKinematicsSolver{ +// protected: +// hrp::BodyPtr _robot_copy; +// pthread_t ik_thread; +// pthread_mutex_t ik_body_mutex; // Mutex +// int cur_ik_loop; +// std::vector ikc_list; +// bool is_first; +// bool ik_thread_kill; +// bool ik_thread_ik_required; +// hrp::Vector3 base_p_old; +// hrp::Matrix33 base_R_old; +// hrp::dvector q_old; +// public: +// FullbodyInverseKinematicsSolverMT (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) : FullbodyInverseKinematicsSolver(_robot,_print_str, _dt) { +// _robot_copy = hrp::BodyPtr(new hrp::Body(*_robot)); +// cur_ik_loop = 0; +// is_first = true; +// ik_thread_kill = false; +// ik_thread_ik_required = false; +// }; +// ~FullbodyInverseKinematicsSolverMT () { +// pthread_cancel(ik_thread); +// ik_thread_kill = true; +// pthread_join(ik_thread, NULL ); +// std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread killed successfully"<& _ikc_list, const int _max_iteration) { +// if(is_first){ +// for(int i=0;ijoint(i)->q = _robot->joint(i)->q; +// _robot_copy->joint(i)->ulimit = _robot->joint(i)->ulimit; +// _robot_copy->joint(i)->llimit = _robot->joint(i)->llimit; +// } +// _robot_copy->rootLink()->p = _robot->rootLink()->p; +// _robot_copy->rootLink()->R = _robot->rootLink()->R; +// pthread_mutex_init( &ik_body_mutex, NULL ); +// pthread_create(&ik_thread, NULL, launchThread, this); +// is_first = false; +// } +// for(int i=0;ijoint(i)->q = _robot_copy->joint(i)->q; +// } +// _robot->rootLink()->p = _robot_copy->rootLink()->p; +// _robot->rootLink()->R = _robot_copy->rootLink()->R; +// _robot->calcForwardKinematics(); +// int result = cur_ik_loop; +// +// if(pthread_mutex_lock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_lock err "<rootLink()->p; +// base_R_old = _robot_copy->rootLink()->R; +// q_old.resize(J_DOF); +// for(int i=0;ijoint(i)->q; } +// +// cur_ik_loop = 0; +// ikc_list = _ikc_list; +// ik_thread_ik_required = true; +// if(pthread_mutex_unlock( &ik_body_mutex ) != 0){std::cerr<<"[FullbodyInverseKinematicsSolverMT] pthread_mutex_unlock err "<rootLink()->v = (_robot_copy->rootLink()->p - base_p_old)/ m_dt; +// _robot_copy->rootLink()->w = base_R_old * omegaFromRotEx(base_R_old.transpose() * _robot_copy->rootLink()->R) / m_dt; +// for(int i=0;ijoint(i)->dq = (_robot_copy->joint(i)->q - q_old(i)) / m_dt; } +// _robot_copy->calcForwardKinematics(true,false); +//// _robot_copy->calcTotalMomentum(cur_momentum_P, cur_momentum_L); +// cur_ik_loop++; +// if(cur_ik_loop > 10000){std::cerr<<"[FullbodyInverseKinematicsSolverMT] thread time out"<(pParam)->ik_loop(); +// return NULL; +// } +//}; + + +#endif // FULLBODYIK_H + diff --git a/rtc/AutoBalancer/GaitGenerator.cpp b/rtc/AutoBalancer/GaitGenerator.cpp index 1be6d4390a9..a8a3d0a4c17 100644 --- a/rtc/AutoBalancer/GaitGenerator.cpp +++ b/rtc/AutoBalancer/GaitGenerator.cpp @@ -34,7 +34,7 @@ namespace rats u(2), v(2), 1; ret = dvm * cycloid_point + start + uz; }; - void multi_mid_coords (coordinates& ret, const std::vector& cs) + void multi_mid_coords (coordinates& ret, const std::vector& cs, const double eps) { if (cs.size() == 1) { ret = cs.front(); @@ -43,14 +43,33 @@ namespace rats double ratio = (1.0 - 1.0 / cs.size()); for (size_t i = 1; i < cs.size(); i++) { coordinates tmp; - mid_coords(tmp, ratio, cs.front(), cs.at(i)); + mid_coords(tmp, ratio, cs.front(), cs.at(i), eps); tmp_mid_coords.push_back(tmp); } - multi_mid_coords(ret, tmp_mid_coords); + multi_mid_coords(ret, tmp_mid_coords, eps); } return; }; + std::string leg_type_to_leg_type_string (const leg_type l_r) + { + return ((l_r==LLEG)?std::string("lleg"): + (l_r==RARM)?std::string("rarm"): + (l_r==LARM)?std::string("larm"): + std::string("rleg")); + }; + + double set_value_according_to_toe_heel_type (const toe_heel_type tht, const double toe_value, const double heel_value, const double default_value) + { + if (tht == TOE) { + return toe_value; + } else if (tht == HEEL) { + return heel_value; + } else { + return default_value; + } + }; + /* member function implementation for refzmp_generator */ void refzmp_generator::push_refzmp_from_footstep_nodes_for_dual (const std::vector& fns, const std::vector& _support_leg_steps, @@ -80,10 +99,11 @@ namespace rats } swing_leg_types_list.push_back( swing_leg_types ); step_count_list.push_back(static_cast(fns.front().step_time/dt)); + toe_heel_types_list.push_back(toe_heel_types(SOLE, SOLE)); //std::cerr << "double " << (fns[fs_index].l_r==RLEG?LLEG:RLEG) << " [" << refzmp_cur_list.back()(0) << " " << refzmp_cur_list.back()(1) << " " << refzmp_cur_list.back()(2) << "]" << std::endl; }; - void refzmp_generator::push_refzmp_from_footstep_nodes_for_single (const std::vector& fns, const std::vector& _support_leg_steps) + void refzmp_generator::push_refzmp_from_footstep_nodes_for_single (const std::vector& fns, const std::vector& _support_leg_steps, const toe_heel_types& tht) { // support leg = prev fns l_r // swing leg = fns l_r @@ -106,14 +126,15 @@ namespace rats } swing_leg_types_list.push_back( swing_leg_types ); step_count_list.push_back(static_cast(fns.front().step_time/dt)); + toe_heel_types_list.push_back(tht); //std::cerr << "single " << fns[fs_index-1].l_r << " [" << refzmp_cur_list.back()(0) << " " << refzmp_cur_list.back()(1) << " " << refzmp_cur_list.back()(2) << "]" << std::endl; }; void refzmp_generator::calc_current_refzmp (hrp::Vector3& ret, std::vector& swing_foot_zmp_offsets, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const double default_double_support_static_ratio_before, const double default_double_support_static_ratio_after) { size_t cnt = one_step_count - refzmp_count; // current counter (0 -> one_step_count) - size_t double_support_count_half_before = default_double_support_ratio_before * one_step_count; - size_t double_support_count_half_after = default_double_support_ratio_after * one_step_count; + size_t double_support_count_half_before = default_double_support_ratio_before * one_step_count + 1; + size_t double_support_count_half_after = default_double_support_ratio_after * one_step_count + 1; size_t double_support_static_count_half_before = default_double_support_static_ratio_before * one_step_count; size_t double_support_static_count_half_after = default_double_support_static_ratio_after * one_step_count; for (size_t i = 0; i < swing_leg_types_list[refzmp_index].size(); i++) { @@ -125,19 +146,27 @@ namespace rats // Calculate swing foot zmp offset for toe heel zmp transition if (use_toe_heel_transition && !(is_start_double_support_phase() || is_end_double_support_phase())) { // Do not use toe heel zmp transition during start and end double support period because there is no swing foot - if (thp_ptr->is_between_phases(cnt, SOLE0)) { - double ratio = thp_ptr->calc_phase_ratio(cnt+1, SOLE0); - swing_foot_zmp_offsets.front()(0) = (1-ratio)*swing_foot_zmp_offsets.front()(0) + ratio*toe_zmp_offset_x; - } else if (thp_ptr->is_between_phases(cnt, HEEL2SOLE, SOLE2)) { - double ratio = thp_ptr->calc_phase_ratio(cnt, HEEL2SOLE, SOLE2); - swing_foot_zmp_offsets.front()(0) = ratio*swing_foot_zmp_offsets.front()(0) + (1-ratio)*heel_zmp_offset_x; - } else if (thp_ptr->is_between_phases(cnt, SOLE0, SOLE2TOE)) { - swing_foot_zmp_offsets.front()(0) = toe_zmp_offset_x; - } else if (thp_ptr->is_between_phases(cnt, SOLE2HEEL, HEEL2SOLE)) { - swing_foot_zmp_offsets.front()(0) = heel_zmp_offset_x; - } else if (thp_ptr->is_between_phases(cnt, SOLE2TOE, SOLE2HEEL)) { - double ratio = thp_ptr->calc_phase_ratio(cnt, SOLE2TOE, SOLE2HEEL); - swing_foot_zmp_offsets.front()(0) = ratio * heel_zmp_offset_x + (1-ratio) * toe_zmp_offset_x; + double first_zmp_offset_x, second_zmp_offset_x; + if (use_toe_heel_auto_set) { + first_zmp_offset_x = set_value_according_to_toe_heel_type(toe_heel_types_list[refzmp_index].src_type, toe_zmp_offset_x, heel_zmp_offset_x, swing_foot_zmp_offsets.front()(0)); + second_zmp_offset_x = swing_foot_zmp_offsets.front()(0); + } else { + first_zmp_offset_x = toe_zmp_offset_x; + second_zmp_offset_x = heel_zmp_offset_x; + } + if (thp.is_between_phases(cnt, SOLE0)) { + double ratio = thp.calc_phase_ratio(cnt+1, SOLE0); + swing_foot_zmp_offsets.front()(0) = (1-ratio)*swing_foot_zmp_offsets.front()(0) + ratio*first_zmp_offset_x; + } else if (thp.is_between_phases(cnt, HEEL2SOLE, SOLE2)) { + double ratio = thp.calc_phase_ratio(cnt, HEEL2SOLE, SOLE2); + swing_foot_zmp_offsets.front()(0) = ratio*swing_foot_zmp_offsets.front()(0) + (1-ratio)*second_zmp_offset_x; + } else if (thp.is_between_phases(cnt, SOLE0, SOLE2TOE)) { + swing_foot_zmp_offsets.front()(0) = first_zmp_offset_x; + } else if (thp.is_between_phases(cnt, SOLE2HEEL, HEEL2SOLE)) { + swing_foot_zmp_offsets.front()(0) = second_zmp_offset_x; + } else if (thp.is_between_phases(cnt, SOLE2TOE, SOLE2HEEL)) { + double ratio = thp.calc_phase_ratio(cnt, SOLE2TOE, SOLE2HEEL); + swing_foot_zmp_offsets.front()(0) = ratio * second_zmp_offset_x + (1-ratio) * first_zmp_offset_x; } zmp_diff = swing_foot_zmp_offsets.front()(0)-default_zmp_offsets[swing_leg_types_list[refzmp_index].front()](0); if ((is_second_phase() && ( cnt < double_support_count_half_before )) || @@ -175,39 +204,100 @@ namespace rats } }; - void refzmp_generator::update_refzmp (const std::vector< std::vector >& fnsl) + void refzmp_generator::update_refzmp () { if ( 1 <= refzmp_count ) { refzmp_count--; } else { refzmp_index++; - refzmp_count = one_step_count = step_count_list[refzmp_index]; + // Check length of step_count_list and refzmp_index + // The case if !(refzmp_index <= step_count_list.size()-1) is finalizing of gait_generator. + // If finalizing, this can be neglected. + if (refzmp_index <= step_count_list.size()-1) { + refzmp_count = one_step_count = step_count_list[refzmp_index]; + thp.set_one_step_count(one_step_count); + } //std::cerr << "fs " << fs_index << "/" << fnl.size() << " rf " << refzmp_index << "/" << refzmp_cur_list.size() << " flg " << std::endl; } }; + void leg_coords_generator::calc_current_swing_foot_rot (std::map& tmp_swing_foot_rot, const double _default_double_support_ratio_before, const double _default_double_support_ratio_after) + { + // interpolation + int support_len_before = one_step_count * _default_double_support_ratio_before; + int support_len_after = one_step_count * (_default_double_support_ratio_after + swing_rot_count_ratio); + int current_swing_count = (one_step_count - lcg_count); // 0->one_step_count + // swing foot rot interpolator interpolates difference from src to dst. + if (current_swing_count == support_len_before) { + for (std::vector::iterator it = swing_leg_src_steps.begin(); it != swing_leg_src_steps.end(); it++) { + swing_foot_rot_interpolator[it->l_r]->clear(); + double tmp[3] = {}; + swing_foot_rot_interpolator[it->l_r]->set(tmp); + } + int swing_len = one_step_count - support_len_before - support_len_after; + for (size_t ii = 0; ii < swing_leg_dst_steps.size(); ii++) { + leg_type lt = swing_leg_dst_steps[ii].l_r; + swing_foot_rot_interpolator[lt]->setGoal(hrp::rpyFromRot(swing_leg_src_steps[ii].worldcoords.rot.transpose() * swing_leg_dst_steps[ii].worldcoords.rot).data(), + dt * swing_len); + swing_foot_rot_interpolator[lt]->sync(); + } + } else if ( (current_swing_count > support_len_before) && (current_swing_count < (one_step_count-support_len_after) ) ) { + int tmp_len = (lcg_count - support_len_after); + for (size_t ii = 0; ii < swing_leg_dst_steps.size(); ii++) { + leg_type lt = swing_leg_dst_steps[ii].l_r; + swing_foot_rot_interpolator[lt]->setGoal(hrp::rpyFromRot(swing_leg_src_steps[ii].worldcoords.rot.transpose() * swing_leg_dst_steps[ii].worldcoords.rot).data(), + dt * tmp_len); + swing_foot_rot_interpolator[lt]->sync(); + } + } + for (size_t ii = 0; ii < swing_leg_dst_steps.size(); ii++) { + hrp::Vector3 tmpv; + if ( !swing_foot_rot_interpolator[swing_leg_dst_steps[ii].l_r]->isEmpty() ) { + swing_foot_rot_interpolator[swing_leg_dst_steps[ii].l_r]->get(tmpv.data(), true); + } else { + if ( (current_swing_count < support_len_before) ) { + tmpv = hrp::Vector3::Zero(); + } else if (current_swing_count >= (one_step_count-support_len_after)) { + tmpv = hrp::rpyFromRot(swing_leg_src_steps[ii].worldcoords.rot.transpose() * swing_leg_dst_steps[ii].worldcoords.rot); + } + } + tmp_swing_foot_rot.insert(std::pair(swing_leg_dst_steps[ii].l_r, tmpv)); + } + }; + /* member function implementation for leg_coords_generator */ - void leg_coords_generator::calc_current_swing_leg_steps (std::vector& rets, const double step_height, const double _current_toe_angle, const double _current_heel_angle) + void leg_coords_generator::calc_current_swing_leg_steps (std::vector& rets, const double step_height, const double _current_toe_angle, const double _current_heel_angle, const double _default_double_support_ratio_before, const double _default_double_support_ratio_after) { /* match the src step order and the dst step order */ std::sort(swing_leg_src_steps.begin(), swing_leg_src_steps.end(), ((&boost::lambda::_1->* &step_node::l_r) < (&boost::lambda::_2->* &step_node::l_r))); std::sort(swing_leg_dst_steps.begin(), swing_leg_dst_steps.end(), ((&boost::lambda::_1->* &step_node::l_r) < (&boost::lambda::_2->* &step_node::l_r))); + std::map tmp_swing_foot_rot; + calc_current_swing_foot_rot(tmp_swing_foot_rot, _default_double_support_ratio_before, _default_double_support_ratio_after); size_t swing_trajectory_generator_idx = 0; - for (std::vector::iterator it1 = swing_leg_src_steps.begin(), it2 = swing_leg_dst_steps.begin(); - it1 != swing_leg_src_steps.end() && it2 != swing_leg_dst_steps.end(); - it1++, it2++) { + for (std::vector::iterator it1 = swing_leg_src_steps.begin(), it2 = swing_leg_dst_steps.begin(), it3 = current_swing_leg_steps.begin(); + it1 != swing_leg_src_steps.end() && it2 != swing_leg_dst_steps.end() && it3 != current_swing_leg_steps.end(); + it1++, it2++, it3++) { coordinates ret; + ret.rot = it1->worldcoords.rot * hrp::rotFromRpy(tmp_swing_foot_rot[it2->l_r]); + { + // is_touch_ground = false; + // touch_ground_count[it1->l_r == RLEG ? LLEG : RLEG] = 0; + // if (act_contact_states[it1->l_r == RLEG ? 1 : 0]) touch_ground_count[it1->l_r]++; + // int thre_count = static_cast(one_step_count * _default_double_support_ratio_before); // quarter of double support phase + // if (touch_ground_count[it1->l_r] > thre_count || !use_act_states) is_touch_ground = true; + is_touch_ground = true; // more stable without judging ground_touch + } switch (default_orbit_type) { case SHUFFLING: - mid_coords(ret, swing_rot_ratio, it1->worldcoords, it2->worldcoords); + ret.pos = swing_ratio*it1->worldcoords.pos + (1-swing_ratio)*it2->worldcoords.pos; break; case CYCLOID: cycloid_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height); break; case RECTANGLE: - rectangle_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height, swing_trajectory_generator_idx); + rectangle_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height, swing_trajectory_generator_idx, it3->worldcoords, it1->l_r); break; case STAIR: stair_midcoords(ret, it1->worldcoords, it2->worldcoords, step_height); @@ -224,7 +314,7 @@ namespace rats default: break; } swing_trajectory_generator_idx++; - if (std::fabs(step_height) > 1e-3*10) { + if (std::fabs(step_height) > 1e-3*10 && (_current_toe_angle > 0.1 || _current_heel_angle > 0.1)) { if (swing_leg_src_steps.size() == 1) /* only biped or crawl because there is only one toe_heel_interpolator */ modif_foot_coords_for_toe_heel_phase(ret, _current_toe_angle, _current_heel_angle); } @@ -242,30 +332,14 @@ namespace rats double tmp_current_swing_time; int current_swing_count = (one_step_count - lcg_count); // 0->one_step_count if ( current_swing_count < support_len_before ) { // First double support period - swing_ratio = swing_rot_ratio = 0.0; + swing_ratio = 0.0; tmp_current_swing_time = current_swing_len * dt - swing_len * dt; is_swing_phase = false; } else if ( current_swing_count >= support_len_before+swing_len ) { // Last double support period - swing_ratio = swing_rot_ratio = 1.0; + swing_ratio = 1.0; tmp_current_swing_time = current_swing_len * dt + (support_len_before + support_len_after + next_one_step_count) * dt; is_swing_phase = false; } else { - if (current_swing_count == support_len_before) { - double tmp = 0.0; - swing_foot_rot_ratio_interpolator->clear(); - swing_foot_rot_ratio_interpolator->set(&tmp); - tmp = 1.0; - // int reduced_swing_len = 0.95*swing_len; // For margin from early landing - // swing_foot_rot_ratio_interpolator->go(&tmp, dt * reduced_swing_len); - //swing_foot_rot_ratio_interpolator->go(&tmp, dt * swing_len); - swing_foot_rot_ratio_interpolator->setGoal(&tmp, dt * swing_len); - swing_foot_rot_ratio_interpolator->sync(); - } - if (!swing_foot_rot_ratio_interpolator->isEmpty()) { - swing_foot_rot_ratio_interpolator->get(&swing_rot_ratio, true); - } else { - swing_foot_rot_ratio_interpolator->get(&swing_rot_ratio, false); - } tmp_current_swing_time = current_swing_len * dt; swing_ratio = static_cast(current_swing_count-support_len_before)/swing_len; //std::cerr << "gp " << swing_ratio << " " << swing_rot_ratio << std::endl; @@ -289,11 +363,11 @@ namespace rats { double tmp_ip_ratio; size_t current_count = one_step_count - lcg_count; - if (thp_ptr->is_phase_starting(current_count, start_phase)) { + if (thp.is_phase_starting(current_count, start_phase)) { toe_heel_interpolator->clear(); toe_heel_interpolator->set(&start); - //toe_heel_interpolator->go(&goal, thp_ptr->calc_phase_period(start_phase, goal_phase, dt)); - toe_heel_interpolator->setGoal(&goal, thp_ptr->calc_phase_period(start_phase, goal_phase, dt)); + //toe_heel_interpolator->go(&goal, thp.calc_phase_period(start_phase, goal_phase, dt)); + toe_heel_interpolator->setGoal(&goal, thp.calc_phase_period(start_phase, goal_phase, dt)); toe_heel_interpolator->sync(); } if (!toe_heel_interpolator->isEmpty()) { @@ -310,32 +384,47 @@ namespace rats size_t current_count = one_step_count - lcg_count; double dif_angle = 0.0; hrp::Vector3 ee_local_pivot_pos(hrp::Vector3(0,0,0)); - if ( thp_ptr->is_between_phases(current_count, SOLE0, SOLE2TOE) ) { - dif_angle = calc_interpolated_toe_heel_angle(SOLE0, SOLE2TOE, 0.0, _current_toe_angle); - ee_local_pivot_pos(0) = toe_pos_offset_x; - } else if ( thp_ptr->is_between_phases(current_count, SOLE2HEEL, HEEL2SOLE) ) { - dif_angle = calc_interpolated_toe_heel_angle(SOLE2HEEL, HEEL2SOLE, -1 * _current_heel_angle, 0.0); - ee_local_pivot_pos(0) = heel_pos_offset_x; - } else if ( thp_ptr->is_between_phases(current_count, SOLE2TOE, SOLE2HEEL) ) { + double first_goal_angle, second_goal_angle, first_pos_offset_x, second_pos_offset_x; + if (use_toe_heel_auto_set) { + first_goal_angle = set_value_according_to_toe_heel_type(current_src_toe_heel_type, _current_toe_angle, -1 * _current_heel_angle, 0); + second_goal_angle = 0; + first_pos_offset_x = set_value_according_to_toe_heel_type(current_src_toe_heel_type, toe_pos_offset_x, heel_pos_offset_x, 0); + second_pos_offset_x = 0; + } else { + first_goal_angle = _current_toe_angle; + second_goal_angle = -1 * _current_heel_angle; + first_pos_offset_x = toe_pos_offset_x; + second_pos_offset_x = heel_pos_offset_x; + } + if ( thp.is_between_phases(current_count, SOLE0, SOLE2TOE) ) { + dif_angle = calc_interpolated_toe_heel_angle(SOLE0, SOLE2TOE, 0.0, first_goal_angle); + ee_local_pivot_pos(0) = first_pos_offset_x; + } else if ( thp.is_between_phases(current_count, SOLE2HEEL, HEEL2SOLE) ) { + dif_angle = calc_interpolated_toe_heel_angle(SOLE2HEEL, HEEL2SOLE, second_goal_angle, 0.0); + ee_local_pivot_pos(0) = second_pos_offset_x; + } else if ( thp.is_between_phases(current_count, SOLE2TOE, SOLE2HEEL) ) { // If SOLE1 phase does not exist, interpolate toe => heel smoothly, without 0 velocity phase. - if ( thp_ptr->is_no_SOLE1_phase() ) { - dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, SOLE2HEEL, _current_toe_angle, -1 * _current_heel_angle); - double tmpd = (-1*_current_heel_angle-_current_toe_angle); + if ( thp.is_no_SOLE1_phase() ) { + dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, SOLE2HEEL, first_goal_angle, second_goal_angle); + double tmpd = (second_goal_angle-first_goal_angle); if (std::fabs(tmpd) > 1e-5) { - ee_local_pivot_pos(0) = (heel_pos_offset_x - toe_pos_offset_x) * (dif_angle - _current_toe_angle) / tmpd + toe_pos_offset_x; + ee_local_pivot_pos(0) = (second_pos_offset_x - first_pos_offset_x) * (dif_angle - first_goal_angle) / tmpd + first_pos_offset_x; + } else { + ee_local_pivot_pos(0) = first_pos_offset_x; } } else { - if ( thp_ptr->is_between_phases(current_count, SOLE2TOE, TOE2SOLE) ) { - dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, TOE2SOLE, _current_toe_angle, 0.0); - ee_local_pivot_pos(0) = toe_pos_offset_x; - } else if ( thp_ptr->is_between_phases(current_count, SOLE1, SOLE2HEEL) ) { - dif_angle = calc_interpolated_toe_heel_angle(SOLE1, SOLE2HEEL, 0.0, -1 * _current_heel_angle); - ee_local_pivot_pos(0) = heel_pos_offset_x; + if ( thp.is_between_phases(current_count, SOLE2TOE, TOE2SOLE) ) { + dif_angle = calc_interpolated_toe_heel_angle(SOLE2TOE, TOE2SOLE, first_goal_angle, 0.0); + ee_local_pivot_pos(0) = first_pos_offset_x; + } else if ( thp.is_between_phases(current_count, SOLE1, SOLE2HEEL) ) { + dif_angle = calc_interpolated_toe_heel_angle(SOLE1, SOLE2HEEL, 0.0, second_goal_angle); + ee_local_pivot_pos(0) = second_pos_offset_x; } } } foot_dif_rot_angle = (dif_angle > 0.0 ? deg2rad(dif_angle) : 0.0); if (use_toe_joint && dif_angle > 0.0) dif_angle = 0.0; + toe_heel_dif_angle = dif_angle; Eigen::AngleAxis tmpr(deg2rad(dif_angle), hrp::Vector3::UnitY()); rotm3times(new_coords.rot, org_coords.rot, tmpr.toRotationMatrix()); new_coords.pos = org_coords.pos + org_coords.rot * ee_local_pivot_pos - new_coords.rot * ee_local_pivot_pos; @@ -345,35 +434,36 @@ namespace rats void leg_coords_generator::cycloid_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height) const { - mid_coords(ret, swing_rot_ratio, start, goal); cycloid_midpoint (ret.pos, swing_ratio, start.pos, goal.pos, height, default_top_ratio); }; void leg_coords_generator::rectangle_midcoords (coordinates& ret, const coordinates& start, - const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx) + const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx, const coordinates& current_coords, leg_type lr) { - mid_coords(ret, swing_rot_ratio, start, goal); - rdtg[swing_trajectory_generator_idx].get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height); + rdtg[swing_trajectory_generator_idx].set_swing_leg(lr); + rdtg[swing_trajectory_generator_idx].set_rectangle_trajectory_way_point_offset(rectangle_way_point_offset); + rdtg[swing_trajectory_generator_idx].is_touch_ground = is_touch_ground; + rdtg[swing_trajectory_generator_idx].is_single_walking = is_single_walking; + if (use_act_states && is_stop_early_foot && use_force_sensor) rdtg[swing_trajectory_generator_idx].goal_off = rectangle_goal_off; + rdtg[swing_trajectory_generator_idx].time_smooth_offset = rectangle_time_smooth_offset; + rdtg[swing_trajectory_generator_idx].get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height, hrp::Vector3(current_coords.pos)); }; void leg_coords_generator::stair_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height) { - mid_coords(ret, swing_rot_ratio, start, goal); sdtg.get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height); }; void leg_coords_generator::cycloid_delay_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx) { - mid_coords(ret, swing_rot_ratio, start, goal); cdtg[swing_trajectory_generator_idx].get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height); }; void leg_coords_generator::cycloid_delay_kick_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height) { - mid_coords(ret, swing_rot_ratio, start, goal); cdktg.set_start_rot(hrp::Matrix33(start.rot)); cdktg.get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height); }; @@ -381,7 +471,6 @@ namespace rats void leg_coords_generator::cross_delay_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height, leg_type lr) { - mid_coords(ret, swing_rot_ratio, start, goal); crdtg.set_swing_leg(lr); crdtg.get_trajectory_point(ret.pos, hrp::Vector3(start.pos), hrp::Vector3(goal.pos), height); }; @@ -403,48 +492,80 @@ namespace rats return matching_flag; }; - void leg_coords_generator::update_leg_steps (const std::vector< std::vector >& fnsl, const double default_double_support_ratio_before, const double default_double_support_ratio_after) + void leg_coords_generator::calc_swing_support_mid_coords () { - if (!foot_ratio_interpolator->isEmpty()) { - foot_ratio_interpolator->get(&foot_midcoords_ratio, true); - } + std::vector swg_src_coords, swg_dst_coords,sup_coords; + for (std::vector::const_iterator it = swing_leg_src_steps.begin(); it != swing_leg_src_steps.end(); it++) { + if (it->l_r == RLEG or it->l_r == LLEG) swg_src_coords.push_back(it->worldcoords); + } + for (std::vector::const_iterator it = swing_leg_dst_steps.begin(); it != swing_leg_dst_steps.end(); it++) { + if (it->l_r == RLEG or it->l_r == LLEG) swg_dst_coords.push_back(it->worldcoords); + } + for (std::vector::const_iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) { + if (it->l_r == RLEG or it->l_r == LLEG) sup_coords.push_back(it->worldcoords); + } + coordinates tmp_swg_src_mid, tmp_swg_dst_mid, tmp_swg_mid, tmp_sup_mid; + const double rot_eps = 1e-5; // eps for mid_rot calculation + if (swg_src_coords.size() > 0) multi_mid_coords(tmp_swg_src_mid, swg_src_coords, rot_eps); + if (swg_dst_coords.size() > 0) multi_mid_coords(tmp_swg_dst_mid, swg_dst_coords, rot_eps); + if (sup_coords.size() > 0) multi_mid_coords(tmp_sup_mid, sup_coords, rot_eps); + if (lcg_count == one_step_count) { + foot_midcoords_interpolator->clear(); + double tmp[foot_midcoords_interpolator->dimension()]; + for (size_t ii = 0; ii < 3; ii++) { + tmp[ii] = tmp_swg_src_mid.pos(ii); + tmp[ii+3] = 0; + } + foot_midcoords_interpolator->set(tmp); + // set dst + hrp::Matrix33 difrot(tmp_swg_src_mid.rot.transpose() * tmp_swg_dst_mid.rot); + hrp::Vector3 tmpr = hrp::rpyFromRot(difrot); + for (size_t ii = 0; ii < 3; ii++) { + tmp[ii] = tmp_swg_dst_mid.pos(ii); + tmp[ii+3] = tmpr(ii); + } + foot_midcoords_interpolator->setGoal(tmp, dt*one_step_count, true); + foot_midcoords_interpolator->sync(); + } else { + double tmp[foot_midcoords_interpolator->dimension()]; + hrp::Matrix33 difrot(tmp_swg_src_mid.rot.transpose() * tmp_swg_dst_mid.rot); + hrp::Vector3 tmpr = hrp::rpyFromRot(difrot); + for (size_t ii = 0; ii < 3; ii++) { + tmp[ii] = tmp_swg_dst_mid.pos(ii); + tmp[ii+3] = tmpr(ii); + } + foot_midcoords_interpolator->setGoal(tmp, dt*lcg_count, true); + foot_midcoords_interpolator->sync(); + } + if (!foot_midcoords_interpolator->isEmpty()) { + double tmp[foot_midcoords_interpolator->dimension()]; + foot_midcoords_interpolator->get(tmp, true); + hrp::Vector3 tmpr; + for (size_t ii = 0; ii < 3; ii++) { + tmp_swg_mid.pos(ii) = tmp[ii]; + tmpr(ii) = tmp[ii+3]; + } + tmp_swg_mid.rot = tmp_swg_src_mid.rot * hrp::rotFromRpy(tmpr); + } else { + tmp_swg_mid = tmp_swg_dst_mid; + } + mid_coords(swing_support_midcoords, static_cast(sup_coords.size()) / (swg_src_coords.size() + sup_coords.size()), tmp_swg_mid, tmp_sup_mid, rot_eps); + }; + void leg_coords_generator::update_leg_steps (const std::vector< std::vector >& fnsl, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const toe_heel_type_checker& thtc) + { // Get current swing coords, support coords, and support leg parameters - size_t current_footstep_index = (footstep_index < fnsl.size() - 1 ? footstep_index : fnsl.size()-1); - swing_leg_dst_steps = fnsl[current_footstep_index]; - if (footstep_index != 0) { // If not initial step, support_leg_coords is previous swing_leg_dst_coords // why we need this? - support_leg_steps = support_leg_steps_list[current_footstep_index]; - } - support_leg_types.clear(); - for (std::vector::iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) { - support_leg_types.push_back(it->l_r); - } - swing_leg_types.clear(); - for (std::vector::iterator it = swing_leg_dst_steps.begin(); it != swing_leg_dst_steps.end(); it++) { - swing_leg_types.push_back(it->l_r); - } - if (current_footstep_index > 0) { - if (is_same_footstep_nodes(fnsl[current_footstep_index], fnsl[current_footstep_index-1])) { - swing_leg_src_steps = swing_leg_dst_steps_list[current_footstep_index-1]; - } else { - /* current swing leg src coords = (previout support leg coords + previous swing leg dst coords) - current support leg coords */ - std::vector tmp_swing_leg_src_steps = support_leg_steps_list[current_footstep_index-1]; - std::copy(swing_leg_dst_steps_list[current_footstep_index-1].begin(), - swing_leg_dst_steps_list[current_footstep_index-1].end(), - std::back_inserter(tmp_swing_leg_src_steps)); - for (size_t i = 0; i < support_leg_steps.size(); i++) { - std::vector::iterator it = std::remove_if(tmp_swing_leg_src_steps.begin(), tmp_swing_leg_src_steps.end(), (&boost::lambda::_1->* &step_node::l_r == support_leg_steps.at(i).l_r)); - tmp_swing_leg_src_steps.erase(it, tmp_swing_leg_src_steps.end()); - } - swing_leg_src_steps = tmp_swing_leg_src_steps; - } - } + calc_swing_support_params_from_footstep_nodes_list(fnsl); + current_src_toe_heel_type = thtc.check_toe_heel_type_from_swing_support_coords(swing_leg_src_steps.front().worldcoords, support_leg_steps.front().worldcoords, toe_pos_offset_x, heel_pos_offset_x); + current_dst_toe_heel_type = thtc.check_toe_heel_type_from_swing_support_coords(swing_leg_dst_steps.front().worldcoords, support_leg_steps.front().worldcoords, toe_pos_offset_x, heel_pos_offset_x); + calc_swing_support_mid_coords (); calc_ratio_from_double_support_ratio(default_double_support_ratio_before, default_double_support_ratio_after); swing_leg_steps.clear(); - calc_current_swing_leg_steps(swing_leg_steps, current_step_height, current_toe_angle, current_heel_angle); + calc_current_swing_leg_steps(swing_leg_steps, current_step_height, current_toe_angle, current_heel_angle, default_double_support_ratio_before, default_double_support_ratio_after); if ( 1 <= lcg_count ) { lcg_count--; + current_swing_leg_steps = swing_leg_steps; } else { //std::cerr << "gp " << footstep_index << std::endl; if (footstep_index < fnsl.size() - 1) { @@ -459,7 +580,7 @@ namespace rats } if (footstep_index < fnsl.size()) { one_step_count = static_cast(fnsl[footstep_index].front().step_time/dt); - thp_ptr->set_one_step_count(one_step_count); + thp.set_one_step_count(one_step_count); } if (footstep_index + 1 < fnsl.size()) { next_one_step_count = static_cast(fnsl[footstep_index+1].front().step_time/dt); @@ -486,12 +607,12 @@ namespace rats default: break; } - reset_foot_ratio_interpolator(); + current_swing_leg_steps = swing_leg_src_steps; } }; /* member function implementation for gait_generator */ - void gait_generator::initialize_gait_parameter (const hrp::Vector3& cog, + void gait_generator::initialize_gait_parameter (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_refcog, const std::vector& initial_support_leg_steps, const std::vector& initial_swing_leg_dst_steps, const double delay) @@ -524,77 +645,379 @@ namespace rats delete preview_controller_ptr; preview_controller_ptr = NULL; } - //preview_controller_ptr = new preview_dynamics_filter(dt, cog(2) - refzmp_cur_list[0](2), refzmp_cur_list[0]); - preview_controller_ptr = new preview_dynamics_filter(dt, cog(2) - rg.get_refzmp_cur()(2), rg.get_refzmp_cur(), gravitational_acceleration); + //preview_controller_ptr = new preview_dynamics_filter(dt, cur_cog(2) - refzmp_cur_list[0](2), refzmp_cur_list[0]); + preview_controller_ptr = new preview_dynamics_filter(dt, cur_cog(2) - rg.get_refzmp_cur()(2), rg.get_refzmp_cur(), gravitational_acceleration); + if ( foot_guided_controller_ptr != NULL ) { + delete foot_guided_controller_ptr; + foot_guided_controller_ptr = NULL; + } + foot_guided_controller_ptr = new foot_guided_controller<3>(dt, cur_refcog(2) - rg.get_refzmp_cur()(2), cur_refcog, total_mass, fg_zmp_cutoff_freq, total_external_force_z, gravitational_acceleration); + // foot_guided_controller_ptr->set_act_vel_ratio(act_vel_ratio); + foot_guided_controller_ptr->set_dc_off(hrp::Vector3::Zero()); + is_first_count = false; + prev_short_of_zmp = hrp::Vector3::Zero(); + prev_use_roll_flywheel = false; + prev_use_pitch_flywheel = false; + prev_act_cp = cur_cog; // assume that robot is stopping when starting walking + fxy = hrp::Vector3::Zero(); + sum_fx = sum_fy = hrp::Vector3::Zero(); + fx_count = fy_count = 0; + prev_fg_ref_zmp = rg.get_refzmp_cur(); + // fx_filter->reset(hrp::Vector3::Zero()); // not used for now + zmp_filter->reset(rg.get_refzmp_cur()); + // double omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / (cur_cog - rg.get_refzmp_cur())(2)); + cp_filter->reset(cur_cog); // assume that robot is stopping when starting walking + lr_region[0] = lr_region[1] = false; + is_emergency_touch_wall = false; + is_stuck = false; lcg.reset(one_step_len, footstep_nodes_list.at(1).front().step_time/dt, initial_swing_leg_dst_steps, initial_swing_leg_dst_steps, initial_support_leg_steps, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after); + if (!double_support_zmp_interpolator->isEmpty()) double_support_zmp_interpolator->clear(); /* make another */ lcg.set_swing_support_steps_list(footstep_nodes_list); for (size_t i = 1; i < footstep_nodes_list.size()-1; i++) { - rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list.at(i), lcg.get_support_leg_steps_idx(i)); + std::vector tmp_swing_leg_src_steps; + lcg.calc_swing_leg_src_steps(tmp_swing_leg_src_steps, footstep_nodes_list, i); + toe_heel_types tht(thtc.check_toe_heel_type_from_swing_support_coords(tmp_swing_leg_src_steps.front().worldcoords, lcg.get_support_leg_steps_idx(i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x()), + thtc.check_toe_heel_type_from_swing_support_coords(lcg.get_swing_leg_dst_steps_idx(i).front().worldcoords, lcg.get_support_leg_steps_idx(i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x())); + rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list.at(i), lcg.get_support_leg_steps_idx(i), tht); } rg.push_refzmp_from_footstep_nodes_for_dual(footstep_nodes_list.back(), lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1), lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1)); emergency_flg = IDLING; + is_enlarged_final_time_for_wheel = false; }; - bool gait_generator::proc_one_tick () + void gait_generator::initialize_jump_parameter (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_refcog, + const std::vector& initial_support_leg_steps, + const std::vector& initial_swing_leg_dst_steps, + const coordinates& start_ref_coords, + const hrp::Vector3& trans, + const double t_squat, const double t_flight) + { + initial_jump_midcoords = jump_midcoords = start_ref_coords; + initial_support_leg = initial_support_leg_steps.front(); + initial_swing_leg = initial_swing_leg_dst_steps.front(); + initial_jump_cog = cur_cog; + jump_phase = BEFORE_JUMP; + + hrp::Vector3 zmp_off = 0.5 * (rg.get_default_zmp_offset(RLEG) + rg.get_default_zmp_offset(LLEG)); + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Matrix33 mid_rot; + calc_foot_origin_rot(mid_rot, initial_jump_midcoords.rot, ez); + hrp::Vector3 tmp_trans = mid_rot * trans; + jump_last_cp = cur_cog + tmp_trans + jump_midcoords.rot * zmp_off; + jump_remain_time = t_squat; + jump_takeoff_height = cur_cog(2); + jump_landing_height = jump_last_cp(2); + jump_flight_time = t_flight; + jump_recover_time = t_squat; + d_jump_pos = tmp_trans; + d_jump_foot_pos = hrp::Vector3::Zero(); + + if ( foot_guided_controller_ptr != NULL ) { + delete foot_guided_controller_ptr; + foot_guided_controller_ptr = NULL; + } + foot_guided_controller_ptr = new foot_guided_controller<3>(dt, cur_refcog(2) - start_ref_coords.pos(2), cur_refcog, total_mass, fg_zmp_cutoff_freq, total_external_force_z, gravitational_acceleration); + foot_guided_controller_ptr->set_dc_off(hrp::Vector3::Zero()); + if (!double_support_zmp_interpolator->isEmpty()) double_support_zmp_interpolator->clear(); + } + + void gait_generator::initialize_wheel_parameter (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_refcog, + const std::vector& initial_support_leg_steps, + const std::vector& initial_swing_leg_dst_steps) + { + wheel_major_index = 0; + wheel_index = 0; + start_wheel_pos_x = 0.0; + if (!wheel_interpolator->isEmpty()) wheel_interpolator->clear(); + double tmp = 0.0; + wheel_interpolator->set(&tmp); + tmp = 1.0; + wheel_interpolator->setGoal(&tmp, wheel_nodes_list.at(wheel_major_index).at(1).time, true); + + initial_wheel_midcoords = wheel_nodes_list.at(wheel_major_index).at(0).worldcoords; + initial_support_leg = initial_support_leg_steps.front(); + initial_swing_leg = initial_swing_leg_dst_steps.front(); + d_wheel_pos = hrp::Vector3::Zero(); + final_footstep_pos = hrp::Vector3::Zero(); + + if ( foot_guided_controller_ptr != NULL ) { + delete foot_guided_controller_ptr; + foot_guided_controller_ptr = NULL; + } + foot_guided_controller_ptr = new foot_guided_controller<3>(dt, cur_refcog(2) - initial_wheel_midcoords.pos(2), cur_refcog, total_mass, fg_zmp_cutoff_freq, total_external_force_z, gravitational_acceleration); + foot_guided_controller_ptr->set_dc_off(hrp::Vector3::Zero()); + if (!double_support_zmp_interpolator->isEmpty()) double_support_zmp_interpolator->clear(); + } + + bool gait_generator::proc_one_tick (hrp::Vector3 cur_cog, hrp::Vector3 cur_cogvel, const hrp::Vector3& target_cog) { solved = false; + if (lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt) - 1) { // for go-velocity + modified_d_footstep = hrp::Vector3::Zero(); + modified_d_step_time = 0.0; + updated_vel_footsteps = false; + is_after_double_support_phase = false; + was_enlarged_time = false; + } + lcg.set_is_single_walking(footstep_nodes_list.size()); + hrp::Vector3 cur_refcog, cur_refcogvel, dc_off; + foot_guided_controller_ptr->get_pos(cur_refcog); + foot_guided_controller_ptr->get_vel(cur_refcogvel); + foot_guided_controller_ptr->get_dc_off(dc_off); + cur_cog -= dc_off; /* update refzmp */ if (emergency_flg == EMERGENCY_STOP && lcg.get_footstep_index() > 0) { leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r; - overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(cur_leg==RLEG?LLEG:RLEG, footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords, 0, default_step_time, 0, 0))); - overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(cur_leg, footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords, 0, default_step_time, 0, 0))); - overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(cur_leg==RLEG?LLEG:RLEG, footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords, 0, default_step_time, 0, 0))); - overwrite_refzmp_queue(overwrite_footstep_nodes_list); + leg_type first_step = overwritable_footstep_index_offset % 2 == 0 ? cur_leg : (cur_leg == RLEG ? LLEG : RLEG); + + overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(first_step, footstep_nodes_list[get_overwritable_index()].front().worldcoords, 0, default_step_time, 0, 0))); + overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(first_step==RLEG?LLEG:RLEG, footstep_nodes_list[get_overwritable_index() - 1].front().worldcoords, 0, default_step_time, 0, 0))); + overwrite_footstep_nodes_list.push_back(boost::assign::list_of(step_node(first_step, footstep_nodes_list[get_overwritable_index()].front().worldcoords, 0, default_step_time, 0, 0))); + + overwrite_refzmp_queue(overwrite_footstep_nodes_list, cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); overwrite_footstep_nodes_list.clear(); emergency_flg = STOPPING; - } else if ( lcg.get_lcg_count() == get_overwrite_check_timing() ) { + } else if ( lcg.get_lcg_count() <= get_overwrite_check_timing() && !updated_vel_footsteps ) { if (velocity_mode_flg != VEL_IDLING && lcg.get_footstep_index() > 0) { std::vector< std::vector > cv; calc_next_coords_velocity_mode(cv, get_overwritable_index(), - (overwritable_footstep_index_offset == 0 ? 4 : 3) // Why? + (5 - overwritable_footstep_index_offset) // assume overwritable_footstep_index_offset < 5 ); - if (velocity_mode_flg == VEL_ENDING) velocity_mode_flg = VEL_IDLING; - std::vector first_overwrite_leg; - for (size_t i = 0; i < footstep_nodes_list[get_overwritable_index()].size(); i++) { - first_overwrite_leg.push_back(footstep_nodes_list[get_overwritable_index()].at(i).l_r); + if (velocity_mode_flg == VEL_ENDING) { + velocity_mode_flg = VEL_IDLING; + if (is_stable_go_stop_mode) is_emergency_step = true; + } + for (size_t i = 0; i < overwritable_footstep_index_offset; i++) { + overwrite_footstep_nodes_list.push_back(footstep_nodes_list[lcg.get_footstep_index() + i]); } for (size_t i = 0; i < cv.size(); i++) { std::vector tmp_fsn; for (size_t j = 0; j < cv.at(i).size(); j++) { + if (overwritable_footstep_index_offset == 0) cv.at(i).at(j).worldcoords.pos += modified_d_footstep; tmp_fsn.push_back(step_node(cv.at(i).at(j).l_r, cv.at(i).at(j).worldcoords, lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle())); + if (overwritable_footstep_index_offset == 0 && i == 0) tmp_fsn.back().step_time += modified_d_step_time; } overwrite_footstep_nodes_list.push_back(tmp_fsn); } - overwrite_refzmp_queue(overwrite_footstep_nodes_list); + overwrite_refzmp_queue(overwrite_footstep_nodes_list, cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); overwrite_footstep_nodes_list.clear(); } else if ( !overwrite_footstep_nodes_list.empty() && // If overwrite_footstep_node_list exists (lcg.get_footstep_index() < footstep_nodes_list.size()-1) && // If overwrite_footstep_node_list is specified and current footstep is not last footstep. get_overwritable_index() == overwrite_footstep_index ) { - overwrite_refzmp_queue(overwrite_footstep_nodes_list); + overwrite_refzmp_queue(overwrite_footstep_nodes_list, cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); overwrite_footstep_nodes_list.clear(); } + updated_vel_footsteps = true; + } + // limit stride + if (use_stride_limitation && lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-overwritable_footstep_index_offset-2 && + (overwritable_footstep_index_offset == 0 || lcg.get_lcg_count() == get_overwrite_check_timing())) { + if (lcg.get_footstep_index() == footstep_nodes_list.size()-overwritable_footstep_index_offset-3) { + hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos; + limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation); + for (size_t i = get_overwritable_index() + 1; i < footstep_nodes_list.size(); i++) { + footstep_nodes_list[i].front().worldcoords.pos -= orig_footstep_pos - footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos; + } + } else { + limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation); + } + overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+get_overwritable_index(), footstep_nodes_list.end()); + overwrite_refzmp_queue(overwrite_footstep_nodes_list); + overwrite_footstep_nodes_list.clear(); } - if ( !solved ) { - hrp::Vector3 rzmp; - std::vector sfzos; - bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after); - if (!refzmp_exist_p) { - finalize_count++; - rzmp = prev_que_rzmp; - sfzos = prev_que_sfzos; + if (is_preview) { + if(modify_footsteps) modify_footsteps_for_recovery(); // modify footsteps based on diff_cp + update_preview_controller(solved); + } else { // foot guided + if (lcg.get_lcg_count() == static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1 && lcg.get_footstep_index() > 0) { + leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r; + lr_region[cur_leg] = false; + if (lr_region[cur_leg == RLEG ? LLEG : RLEG]) { + hrp::Matrix33 prev_fs_rot, preprev_fs_rot; + step_node preprev_fs = (lcg.get_footstep_index()==1 ? lcg.get_swing_leg_src_steps().front() : footstep_nodes_list[lcg.get_footstep_index()-2].front()); + hrp::Vector3 prev_fs_pos = footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos, preprev_fs_pos = preprev_fs.worldcoords.pos; + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + calc_foot_origin_rot(prev_fs_rot, footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.rot, ez); + calc_foot_origin_rot(preprev_fs_rot, preprev_fs.worldcoords.rot, ez); + + // limit stide of second step of stepping up and down on stair + hrp::Vector3 prev_fs_pos_relative = preprev_fs_rot.transpose() * (prev_fs_pos - preprev_fs_pos); // preprev foot frame + double stair_thre = 0.04, up_max_stride = 0.01, down_max_stride = 0.05; + double tmp_forward = overwritable_stride_limitation[0], tmp_rear = overwritable_stride_limitation[3]; + if (prev_fs_pos_relative(2) > stair_thre) { + if (prev_fs_pos_relative(0) > 0.0) overwritable_stride_limitation[0] = up_max_stride; + else overwritable_stride_limitation[3] = up_max_stride; + } else if (prev_fs_pos_relative(2) < - stair_thre) { + if (prev_fs_pos_relative(0) > 0.0) overwritable_stride_limitation[0] = down_max_stride; + else overwritable_stride_limitation[3] = down_max_stride; + } + + if (cur_leg == RLEG) { + stride_limitation_polygon[0] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], -overwritable_stride_limitation[4]-leg_margin[3], 0.0)) - preprev_fs_pos)).head(2); + stride_limitation_polygon[1] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], -overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2); + stride_limitation_polygon[2] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], -overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2); + stride_limitation_polygon[3] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], -overwritable_stride_limitation[4]-leg_margin[3], 0.0)) - preprev_fs_pos)).head(2); + } else { + stride_limitation_polygon[0] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2); + stride_limitation_polygon[1] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(-overwritable_stride_limitation[3], overwritable_stride_limitation[4]+leg_margin[3], 0.0)) - preprev_fs_pos)).head(2); + stride_limitation_polygon[2] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], overwritable_stride_limitation[4]+leg_margin[3], 0.0)) - preprev_fs_pos)).head(2); + stride_limitation_polygon[3] = (preprev_fs_rot.transpose() * ((prev_fs_pos + prev_fs_rot * hrp::Vector3(overwritable_stride_limitation[0], overwritable_stride_limitation[1], 0.0)) - preprev_fs_pos)).head(2); + } + + overwritable_stride_limitation[0] = tmp_forward; + overwritable_stride_limitation[3] = tmp_rear; + + for (size_t i = 0; i < steppable_region[cur_leg == RLEG ? LLEG : RLEG].size(); i++) { + bool is_nan = false; + hrp::Vector2 prev_p = hrp::Vector2::Zero(); + hrp::Vector2 pos_off = hrp::Vector2(0.0, 0.0); + for (size_t j = 0; j < steppable_region[cur_leg == RLEG ? LLEG : RLEG][i].size(); j++) { + if (std::isnan(steppable_region[cur_leg == RLEG ? LLEG : RLEG][i][j](0))) is_nan = true; + if ((prev_p - steppable_region[cur_leg == RLEG ? LLEG : RLEG][i][j]).norm() < 1e-4) is_nan = true; + prev_p = steppable_region[cur_leg == RLEG ? LLEG : RLEG][i][j]; + steppable_region[cur_leg == RLEG ? LLEG : RLEG][i][j] += pos_off; + } + if (is_nan || steppable_region[cur_leg == RLEG ? LLEG : RLEG][i].size() == 0) { + steppable_region[cur_leg == RLEG ? LLEG : RLEG][i].resize(1); + } else { + steppable_region[cur_leg == RLEG ? LLEG : RLEG][i] = calc_intersect_convex(steppable_region[cur_leg == RLEG ? LLEG : RLEG][i], stride_limitation_polygon); + } + } + } + + changed_step_time_stair = false; + was_read_steppable_height = false; + } + // dc fxy + hrp::Vector3 prev_fxy = fxy; + hrp::Vector3 tmp_fxy = hrp::Vector3::Zero(); + double cur_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / foot_guided_controller_ptr->get_dz()); + // double ref_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / (cur_refcog - rg.get_refzmp_cur())(2)); + double ref_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / foot_guided_controller_ptr->get_dz()); + act_cp = cur_cog + cur_cogvel / cur_omega + dc_off; + ref_cp = cur_refcog + cur_refcogvel / ref_omega; + hrp::Vector3 act_cpvel = (act_cp - prev_act_cp) / dt; + if (use_act_states && (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2)) { + tmp_fxy.head(2) = (total_mass * cur_omega * (act_cpvel - cur_omega * (act_cp - refzmp))).head(2); + if (isfinite(tmp_fxy(0)) && isfinite(tmp_fxy(1))) { + sum_fx += tmp_fxy; + sum_fy += tmp_fxy; + fx_count++; + fy_count++; + } + if (lcg.get_lcg_count() == static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1) { // TODO: cannot completely support turn walking + hrp::Matrix33 prev_fs_rot; + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + calc_foot_origin_rot(prev_fs_rot, footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.rot, ez); + sum_fx = prev_fs_rot.transpose() * sum_fx; + if (lcg.get_footstep_index() % 2 == 0) { + sum_fy = prev_fs_rot.transpose() * sum_fy; + des_fxy = hrp::Vector3(sum_fx(0) / fx_count, sum_fy(1) / fy_count, 0.0); + fy_count = 0; + sum_fy = hrp::Vector3::Zero(); + } else { + des_fxy = prev_fs_rot.transpose() * des_fxy; + des_fxy = hrp::Vector3(sum_fx(0) / fx_count, des_fxy(1), 0.0); + } + fx_count = 0; + sum_fx = hrp::Vector3::Zero(); + // TODO: tmp_limit + hrp::Vector3 fx_limit = hrp::Vector3::Zero(); + const double tmp_margin = 1e-2; // 1cm + fx_limit(0) = (safe_leg_margin[0] - tmp_margin) * total_mass * ref_omega * ref_omega; + fx_limit(1) = (safe_leg_margin[2] - tmp_margin) * total_mass * ref_omega * ref_omega; + for (size_t i = 0; i < 2; i++) { + des_fxy(i) = std::min(std::max(des_fxy(i), -fx_limit(i)), fx_limit(i)); + } + des_fxy = prev_fs_rot * des_fxy; + } } else { - prev_que_rzmp = rzmp; - prev_que_sfzos = sfzos; + des_fxy = hrp::Vector3::Zero(); } - solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt)); + tmp[15] = tmp_fxy(0); + tmp[16] = tmp_fxy(1); + // fx = fx_filter->passFilter(fx); + // fxy = prev_fxy + dc_gain * (des_fxy - prev_fxy); + for (size_t i = 0; i < 2; i++) { + double tmp_gain = dc_gain; + if (fabs(des_fxy(i)) < fabs(prev_fxy(i))) tmp_gain *= 10; + fxy(i) = prev_fxy(i) + tmp_gain * (des_fxy(i) - prev_fxy(i)); + } + if (debug_set_landing_height && lcg.get_footstep_index() == 0) debug_orig_height = footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(2); + if (!solved) update_foot_guided_controller(solved, cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); + if (use_act_states && modify_footsteps && (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2)) modify_footsteps_for_foot_guided(cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); + else if (is_emergency_step && lcg.get_footstep_index() == footstep_nodes_list.size()-1) { + is_emergency_step = false; + default_step_time = orig_default_step_time; + default_double_support_ratio_swing_before = orig_default_double_support_static_ratio_before; + default_double_support_ratio_swing_after = orig_default_double_support_static_ratio_after; + min_time = orig_min_time; + } + { + // calc landing pos relative to supporting foot for vision + cur_supporting_foot = (footstep_nodes_list[lcg.get_footstep_index()].front().l_r == LLEG ? 0 : 1); + hrp::Matrix33 cur_rot; + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + step_node cur_sup_fs(lcg.get_footstep_index() > 0 ? footstep_nodes_list[lcg.get_footstep_index()-1].front() : lcg.get_support_leg_steps().front()); + calc_foot_origin_rot(cur_rot, cur_sup_fs.worldcoords.rot, ez); + rel_landing_pos = cur_rot.transpose() * (footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos - cur_sup_fs.worldcoords.pos); + // calc end cog and cogvel relative to supporting foot for touch wall + hrp::Vector3 tmp_cog = cur_rot.transpose() * (cur_cog + dc_off - cur_sup_fs.worldcoords.pos); + hrp::Vector3 tmp_vel = cur_rot.transpose() * cur_cogvel; + hrp::Vector3 tmp_zmp = cur_rot.transpose() * (refzmp - cur_sup_fs.worldcoords.pos); + double remain_time = remain_count * dt; + end_cog = (tmp_cog - tmp_zmp) * std::cosh(cur_omega * remain_time) + tmp_vel / cur_omega * std::sinh(cur_omega * remain_time) + tmp_zmp; + end_cogvel = (tmp_cog - tmp_zmp) * cur_omega * std::sinh(cur_omega * remain_time) + tmp_vel * std::cosh(cur_omega * remain_time); + end_cog(2) = (cur_cog - refzmp)(2); + end_cogvel(2) = 0.0; + } + // judge whehter regenerate motion + if (lcg.get_footstep_index() > 1 && + lcg.get_lcg_count() == static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1) { // reset is_stuck + // minus and x + for (size_t i = 0; i < 1; i++) { + if (sum_d_footstep_minus(i) < - sum_d_footstep_thre(i) && + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(i) < footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos(i) - footstep_check_delta(i)) { + footstep_hist_max(i) = footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos(i); + sum_d_footstep_minus(i) = 0.0; + is_stuck = false; + } + } + } + if (lcg.get_footstep_index() > 5 && + lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (default_double_support_ratio_after)) && + lcg.get_lcg_count() < static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) + ) { + for (size_t i = 0; i < 1; i++) { + // minus + if (footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos(i) > footstep_hist_max(i) + footstep_check_delta(i)) { + footstep_hist_max(i) = footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos(i); + sum_d_footstep_minus(i) = 0.0; + is_stuck = false; + } else if (footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos(i) > footstep_hist_max(i)) { + footstep_hist_max(i) = footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos(i); + } + if (sum_d_footstep_minus(i) < - sum_d_footstep_thre(i) + // || sum_d_footstep_plus(i) > sum_d_footstep_thre(i) + ) { + is_stuck = true; + } + } + } + + prev_act_cp = act_cp; + tmp[17] = fxy(0); + tmp[18] = fxy(1); + tmp[19] = dc_off(0); + tmp[20] = dc_off(1); } - rg.update_refzmp(footstep_nodes_list); + rg.update_refzmp(); // { // debug // double cart_zmp[3]; // preview_controller_ptr->get_cart_zmp(cart_zmp); @@ -607,13 +1030,1192 @@ namespace rats /* update swing_leg_coords, support_leg_coords */ if ( solved ) { - lcg.update_leg_steps(footstep_nodes_list, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after); + lcg.update_leg_steps(footstep_nodes_list, default_double_support_ratio_swing_before, default_double_support_ratio_swing_after, thtc); } else if (finalize_count>0) { lcg.clear_interpolators(); } return solved; }; + bool gait_generator::proc_one_tick_jump (hrp::Vector3 cur_cog, const hrp::Vector3& cur_cogvel, const hrp::Vector3& target_cog) + { + hrp::Vector3 cur_refcog, cur_refcogvel, dc_off; + foot_guided_controller_ptr->get_pos(cur_refcog); + foot_guided_controller_ptr->get_vel(cur_refcogvel); + foot_guided_controller_ptr->get_dc_off(dc_off); + cur_cog -= dc_off; + + // calc_cur_cp + hrp::Vector3 dz = hrp::Vector3(0, 0, (target_cog - jump_midcoords.pos)(2)); + foot_guided_controller_ptr->set_dz(dz(2), total_external_force_z); + // hrp::Vector3 dz = hrp::Vector3(0, 0, foot_guided_controller_ptr->get_dz()); + double cur_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / dz(2)); + double ref_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / dz(2)); + act_cp = cur_cog + cur_cogvel / cur_omega + dc_off; + ref_cp = cur_refcog + cur_refcogvel / ref_omega; + + hrp::Vector3 feedforward_zmp; + foot_guided_controller_ptr->set_x_k(cur_refcog, cur_refcogvel); + if (use_act_states) foot_guided_controller_ptr->set_act_x_k(cur_cog, cur_cogvel, false); + else foot_guided_controller_ptr->set_act_x_k(cur_refcog, cur_refcogvel, false); + + hrp::Vector3 zmp_off = 0.5 * (rg.get_default_zmp_offset(RLEG) + rg.get_default_zmp_offset(LLEG)); + hrp::Vector3 cur_ref_zmp = jump_midcoords.pos + jump_midcoords.rot * zmp_off; + + if (jump_phase == BEFORE_JUMP) { + foot_guided_controller_ptr->update_control_jump(zmp, feedforward_zmp, cur_ref_zmp, jump_last_cp, jump_takeoff_height, jump_flight_time, jump_remain_time); + } else if (jump_phase == JUMPING) { + zmp = feedforward_zmp = cur_refcog; + foot_guided_controller_ptr->set_zmp(zmp, feedforward_zmp); + } else { // AFTER_JUMP + std::vector > ref_zmp_traj; + ref_zmp_traj.reserve(2); + ref_zmp_traj.push_back(LinearTrajectory(cur_ref_zmp, cur_ref_zmp, jump_remain_time)); + ref_zmp_traj.push_back(LinearTrajectory(jump_last_cp - dz, jump_last_cp - dz, 1)); + foot_guided_controller_ptr->update_control(zmp, feedforward_zmp, ref_zmp_traj); + } + hrp::Vector3 refzmp_orig = zmp, feedforward_zmp_orig = feedforward_zmp; + if (jump_phase != JUMPING) { + // project to nominal ground + project_to_nominal_ground(zmp, cur_ref_zmp, cur_cog); + project_to_nominal_ground(feedforward_zmp, cur_ref_zmp, cur_refcog); + // truncate zmp (assume RLEG, LLEG) + Eigen::Vector2d tmp_zmp(zmp.head(2)), tmp_fzmp(feedforward_zmp.head(2)); + if (!is_inside_convex_hull(tmp_zmp, hrp::Vector3::Zero(), true)) { // TODO: should consider footstep rot + zmp.head(2) = tmp_zmp; + } + if (!is_inside_convex_hull(tmp_fzmp, hrp::Vector3::Zero(), true)) { // TODO: should consider footstep rot + feedforward_zmp.head(2) = tmp_fzmp; + } + // revert to zmp height + project_to_nominal_ground(zmp, refzmp_orig, cur_cog); + project_to_nominal_ground(feedforward_zmp, feedforward_zmp_orig, cur_refcog); + foot_guided_controller_ptr->set_zmp(zmp, feedforward_zmp); + } + hrp::Vector3 tmpfxy = hrp::Vector3::Zero(); + foot_guided_controller_ptr->update_state(cog, tmpfxy); + + // convert zmp -> refzmp + refzmp = zmp; + if (jump_phase != JUMPING) { + project_to_nominal_ground(refzmp, cur_ref_zmp, cur_cog); + project_to_nominal_ground(feedforward_zmp, cur_ref_zmp, cur_refcog); + } + + // update foot coords + if (jump_phase == JUMPING || jump_phase == AFTER_JUMP) { + hrp::Vector3 tmp_v; + jump_foot_interpolator->get(tmp_v.data(), true); + d_jump_foot_pos = tmp_v; + } + if (jump_phase == JUMPING) { + d_jump_foot_pos(2) = cog(2) - initial_jump_cog(2); + } + jump_midcoords.pos = initial_jump_midcoords.pos + d_jump_foot_pos; + + tmp[0] = cur_ref_zmp(0); + tmp[1] = cur_ref_zmp(2); + tmp[21] = feedforward_zmp(0); + tmp[22] = feedforward_zmp(2); + tmp[2] = jump_last_cp(0); + tmp[3] = jump_last_cp(2); + tmp[4] = refzmp(0); + tmp[5] = refzmp(2); + tmp[6] = ref_cp(0); + tmp[7] = ref_cp(2); + tmp[8] = act_cp(0); + tmp[9] = act_cp(2); + tmp[10] = feedforward_zmp_orig(0); + tmp[11] = jump_remain_time; + tmp[12] = feedforward_zmp_orig(2); + tmp[13] = refzmp_orig(0); + tmp[14] = refzmp_orig(2); + + + if (jump_phase == JUMPING && jump_remain_time < 0.5*jump_flight_time && + (act_contact_states[0] || act_contact_states[1])) { + jump_remain_time = 0.0; + } + if (jump_remain_time > dt) { + jump_remain_time -= dt; + } else { + hrp::Vector3 tmp_v = hrp::Vector3::Zero(); + switch(jump_phase) { + case BEFORE_JUMP: + jump_remain_time = jump_flight_time; + jump_phase = JUMPING; + if (!jump_foot_interpolator->isEmpty()) jump_foot_interpolator->clear(); + jump_foot_interpolator->set(tmp_v.data()); + tmp_v = d_jump_pos; + jump_foot_interpolator->setGoal(tmp_v.data(), jump_remain_time, true); + break; + case JUMPING: + if (act_contact_states[0] || act_contact_states[1]) { + jump_remain_time = jump_recover_time; + jump_phase = AFTER_JUMP; + jump_foot_interpolator->get(tmp_v.data()); + tmp_v(2) = d_jump_foot_pos(2); + jump_foot_interpolator->set(tmp_v.data()); + tmp_v = d_jump_pos; + jump_foot_interpolator->setGoal(tmp_v.data(), jump_remain_time, true); + } + break; + case AFTER_JUMP: + is_jumping = false; + return false; + break; + default: + break; + } + } + + return true; + } + + bool gait_generator::proc_one_tick_wheel (hrp::Vector3 cur_cog, hrp::Vector3 cur_cogvel, const hrp::Vector3& target_cog) + { + hrp::Vector3 cur_refcog, cur_refcogvel, dc_off; + foot_guided_controller_ptr->get_pos(cur_refcog); + foot_guided_controller_ptr->get_vel(cur_refcogvel); + foot_guided_controller_ptr->get_dc_off(dc_off); + cur_cog -= dc_off; + + int cur_wlist_size = wheel_nodes_list.at(wheel_major_index).size(); + + if (!update_wheel_controller()) return false; + cur_wheel_pos_x -= final_footstep_pos(0); + + hrp::Vector3 dz = hrp::Vector3(0, 0, (target_cog - wheel_midcoords.pos)(2)); + + // calc_cur_cp + { + double cur_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / dz(2)); + double ref_omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / dz(2)); + act_cp = cur_cog + cur_cogvel / cur_omega + dc_off; + ref_cp = cur_refcog + cur_refcogvel / ref_omega; + } + + // calc cog + double remain_time = 0.0; + double traj_remain_time = wheel_interpolator->get_remain_time(); + + foot_guided_controller_ptr->set_dz(dz(2), total_external_force_z); + foot_guided_controller_ptr->set_x_k(cur_refcog, cur_refcogvel); + if (use_act_states) foot_guided_controller_ptr->set_act_x_k(cur_cog, cur_cogvel, false); + else foot_guided_controller_ptr->set_act_x_k(cur_refcog, cur_refcogvel, false); + + hrp::Vector3 zmp_off = 0.5 * (rg.get_default_zmp_offset(RLEG) + rg.get_default_zmp_offset(LLEG)); + hrp::Vector3 cur_ref_zmp = wheel_midcoords.pos + wheel_midcoords.rot * zmp_off; + + std::vector > ref_zmp_traj; + ref_zmp_traj.reserve(cur_wlist_size); + + for (int i = wheel_index; i < cur_wlist_size - 1; i++) { + hrp::Vector3 start = (i == wheel_index ? + cur_ref_zmp : + (wheel_nodes_list.at(wheel_major_index).at(i).worldcoords.pos + wheel_nodes_list.at(wheel_major_index).at(i).worldcoords.rot * zmp_off)); + hrp::Vector3 goal = wheel_nodes_list.at(wheel_major_index).at(i + 1).worldcoords.pos + wheel_nodes_list.at(wheel_major_index).at(i + 1).worldcoords.rot * zmp_off; + double duration = (i == wheel_index ? + traj_remain_time : wheel_nodes_list.at(wheel_major_index).at(i + 1).time); + ref_zmp_traj.push_back(LinearTrajectory(start, goal, duration)); + remain_time += duration; + } + hrp::Vector3 last_cp = ref_zmp_traj.back().getGoal(); + if (wheel_index == cur_wlist_size - 2) { // 余分な直線を最後に追加 + ref_zmp_traj.push_back(LinearTrajectory(last_cp, last_cp, wheel_nodes_list.at(wheel_major_index).at(wheel_index + 1).time)); + remain_time += ref_zmp_traj.back().getTime(); + } + ref_zmp_traj.push_back(LinearTrajectory(last_cp, last_cp, 1)); + + hrp::Vector3 feedforward_zmp; + foot_guided_controller_ptr->update_control(zmp, feedforward_zmp, ref_zmp_traj); + + hrp::Vector3 refzmp_orig = zmp, feedforward_zmp_orig = feedforward_zmp; + { + // project to nominal ground + project_to_nominal_ground(zmp, cur_ref_zmp, cur_cog); + project_to_nominal_ground(feedforward_zmp, cur_ref_zmp, cur_refcog); + // truncate zmp (assume RLEG, LLEG) + Eigen::Vector2d tmp_zmp(zmp.head(2)), tmp_fzmp(feedforward_zmp.head(2)); + if (!is_inside_convex_hull(tmp_zmp, hrp::Vector3::Zero(), true)) { // TODO: should consider footstep rot + zmp.head(2) = tmp_zmp; + } + if (!is_inside_convex_hull(tmp_fzmp, hrp::Vector3::Zero(), true)) { // TODO: should consider footstep rot + feedforward_zmp.head(2) = tmp_fzmp; + } + // revert to zmp height + project_to_nominal_ground(zmp, refzmp_orig, cur_cog); + project_to_nominal_ground(feedforward_zmp, feedforward_zmp_orig, cur_refcog); + // zmp = zmp_filter->passFilter(zmp); + foot_guided_controller_ptr->set_zmp(zmp, feedforward_zmp); + } + // calc cog + hrp::Vector3 tmpfxy = hrp::Vector3::Zero(); + if (use_disturbance_compensation) tmpfxy = fxy; + foot_guided_controller_ptr->update_state(cog, tmpfxy); + // convert zmp -> refzmp + refzmp = zmp; + project_to_nominal_ground(refzmp, cur_ref_zmp, cur_cog); + project_to_nominal_ground(feedforward_zmp, cur_ref_zmp, cur_refcog); + + // for log + tmp[0] = cur_ref_zmp(0); + tmp[1] = cur_ref_zmp(1); + tmp[21] = feedforward_zmp(0); + tmp[22] = feedforward_zmp(1); + tmp[2] = last_cp(0); + tmp[3] = last_cp(1); + tmp[4] = refzmp(0); + tmp[5] = refzmp(1); + tmp[6] = ref_cp(0); + tmp[7] = ref_cp(1); + tmp[8] = act_cp(0); + tmp[9] = act_cp(1); + tmp[10] = traj_remain_time; + tmp[11] = remain_time; + // tmp[12] = flywheel_tau(0); + // tmp[13] = flywheel_tau(1); + // tmp[14] = falling_direction; + } + + bool gait_generator::update_wheel_controller () + { + int cur_wlist_size = wheel_nodes_list.at(wheel_major_index).size(); + + // update_wheel_index + { + if (wheel_interpolator->isEmpty() || wheel_interpolator->get_remain_time() <= dt) { + if (wheel_index >= cur_wlist_size - 2) { + if (wheel_major_index < wheel_nodes_list.size() - 1) { + wheel_major_index++; + wheel_index = -1; + } else { + is_wheeling = false; + return false; + } + } + wheel_index++; + double tmp = 0.0; + wheel_interpolator->set(&tmp); + tmp = 1.0; + wheel_interpolator->setGoal(&tmp, wheel_nodes_list.at(wheel_major_index).at(wheel_index + 1).time, true); + start_wheel_pos_x = cur_wheel_pos_x; + cur_wheel_ratio = 0.0; + } else { + wheel_interpolator->get(&cur_wheel_ratio); + } + } + + // calc_cur_zmp + { + wheel_midcoords.pos = (1.0 - cur_wheel_ratio) * wheel_nodes_list.at(wheel_major_index).at(wheel_index).worldcoords.pos + cur_wheel_ratio * wheel_nodes_list.at(wheel_major_index).at(wheel_index + 1).worldcoords.pos; + wheel_midcoords.rot = wheel_nodes_list.at(wheel_major_index).at(wheel_index).worldcoords.rot; + d_wheel_pos = wheel_midcoords.pos - initial_wheel_midcoords.pos; + } + + // calc_cur_wheel_angle + { + // double goal_pos_x = (wheel_nodes_list.at(wheel_major_index).at(wheel_index).worldcoords.rot.transpose() * (wheel_nodes_list.at(wheel_major_index).at(wheel_index + 1).worldcoords.pos - wheel_nodes_list.at(wheel_major_index).at(wheel_index).worldcoords.pos))(0); + // cur_wheel_pos_x = start_wheel_pos_x + cur_wheel_ratio * goal_pos_x; + cur_wheel_pos_x = d_wheel_pos(0); + } + return true; + } + + void gait_generator::project_to_nominal_ground (hrp::Vector3& p, const hrp::Vector3 ground, const hrp::Vector3 cog) + { + p = cog + ((ground - cog)(2) / (p - cog)(2)) * (p - cog); + } + + void gait_generator::update_preview_controller (bool& solved) + { + if ( !solved ) { + hrp::Vector3 rzmp; + std::vector sfzos; + bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after); + if (!refzmp_exist_p) { + finalize_count++; + rzmp = prev_que_rzmp; + sfzos = prev_que_sfzos; + } else { + prev_que_rzmp = rzmp; + prev_que_sfzos = sfzos; + } + solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt)); + } + } + + void gait_generator::calc_foot_origin_rot (hrp::Matrix33& foot_rot, const hrp::Matrix33& orig_rot, const hrp::Vector3& n = hrp::Vector3::UnitZ()) const + { + hrp::Vector3 en = n / n.norm(); + hrp::Vector3 ex = hrp::Vector3::UnitX(); + hrp::Vector3 xv1(orig_rot * ex); + xv1 = xv1 - xv1.dot(en) * en; + xv1.normalize(); + hrp::Vector3 yv1(en.cross(xv1)); + foot_rot(0,0) = xv1(0); foot_rot(1,0) = xv1(1); foot_rot(2,0) = xv1(2); + foot_rot(0,1) = yv1(0); foot_rot(1,1) = yv1(1); foot_rot(2,1) = yv1(2); + foot_rot(0,2) = en(0); foot_rot(1,2) = en(1); foot_rot(2,2) = en(2); + } + + // TODO: include zmp height to control input + // if not use lcg_count, this function will be more simple + void gait_generator::update_foot_guided_controller (bool& solved, const hrp::Vector3& cur_cog, const hrp::Vector3& cur_cogvel, const hrp::Vector3& cur_refcog, const hrp::Vector3& cur_refcogvel, const hrp::Vector3& target_cog) + { + solved = true; + + size_t step_num(footstep_nodes_list.size()), step_index(lcg.get_footstep_index()); + // for emergency step + if (is_emergency_step && step_index < 3) min_time = std::min(orig_min_time, emergency_step_time[step_index]); + + // if (lcg.get_lcg_count() >= static_cast(footstep_nodes_list[step_index].front().step_time/dt) - 1) + fg_step_count = static_cast(footstep_nodes_list[step_index].front().step_time/dt); + + step_num_phase = NORMAL; + if (step_index == 0) { // first double support phase + step_num_phase = FIRST; + } else if (step_index == 1) { // after first double support phase + step_num_phase = AFTER_FIRST; + } else if (step_index == step_num - 3) { // third to the last double support phase + step_num_phase = BEFORE2_LAST; + } else if (step_index == step_num - 2) { // second to the last double support phase + step_num_phase = BEFORE_LAST; + } else if (step_index == step_num - 1) { // last double support phase + if (is_wheeling && !is_enlarged_final_time_for_wheel && + (wheel_major_index < wheel_nodes_list.size() - 1 || wheel_index < wheel_nodes_list.at(wheel_major_index).size() - 2)) { + is_enlarged_final_time_for_wheel = true; + double w_remain_time = 0.0; + for (int i = wheel_major_index; i < wheel_nodes_list.size(); i++) { + for (int j = (i == wheel_major_index ? wheel_index+1 : 0); j < wheel_nodes_list.at(i).size() - 1; j++) { + if (i == wheel_nodes_list.size() - 1 && j == wheel_nodes_list.at(i).size() - 2) { + w_remain_time += 1; // 最後の直線 + } else { + if (i == wheel_major_index && j == wheel_index+1) w_remain_time += wheel_interpolator->get_remain_time(); + else w_remain_time += wheel_nodes_list.at(i).at(j).time; + } + } + } + change_step_time(w_remain_time - fg_step_count*dt); + fg_step_count = w_remain_time/dt; + } + step_num_phase = LAST; + remain_count = fg_step_count - finalize_count; + if (fg_step_count > finalize_count) { + finalize_count++; + } else { + solved = false; + if (is_wheeling) is_wheeling = false; + } + } + size_t traj_remain_count = lcg.get_lcg_count(); + if (step_num_phase != LAST) remain_count = lcg.get_lcg_count(); + if (step_num_phase != BEFORE_LAST && step_num_phase != LAST) remain_count += static_cast(footstep_nodes_list[step_index + 1].front().step_time/dt * default_double_support_ratio_before) + 2; + walking_phase = SINGLE; + if (lcg.get_lcg_count() >= static_cast(fg_step_count * (1 - default_double_support_ratio_before))) { + walking_phase = DOUBLE_BEFORE; + traj_remain_count -= static_cast(fg_step_count * (1 - default_double_support_ratio_before)); + if (step_num_phase != LAST && step_num_phase != FIRST) remain_count = traj_remain_count; + } else if (lcg.get_lcg_count() <= static_cast(fg_step_count * default_double_support_ratio_after)) { + walking_phase = DOUBLE_AFTER; + } else { + traj_remain_count -= static_cast(fg_step_count * default_double_support_ratio_after) + 1; + } + // remain_count does not become 0 + remain_count++; + traj_remain_count++; + + hrp::Vector3 dz = hrp::Vector3(0, 0, (target_cog - rg.get_refzmp_cur())(2)); + foot_guided_controller_ptr->set_dz(dz(2), total_external_force_z); + foot_guided_controller_ptr->set_x_k(cur_refcog, cur_refcogvel); + if (use_act_states) foot_guided_controller_ptr->set_act_x_k(cur_cog, cur_cogvel, false); + else foot_guided_controller_ptr->set_act_x_k(cur_refcog, cur_refcogvel, false); + + // get cur ref_zmp + std::vector sfzos; // unused + hrp::Vector3 cur_ref_zmp; + bool refzmp_exist_p = rg.get_current_refzmp(cur_ref_zmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after); + if (!refzmp_exist_p) { + cur_ref_zmp = prev_que_rzmp; + } else { + prev_que_rzmp = cur_ref_zmp; + } + + // TODO: num_preview_step > 2もサポートする + std::vector cur_steps(step_index > 0 ? footstep_nodes_list[step_index-1] : lcg.get_support_leg_steps()), dist_steps(footstep_nodes_list[step_index]), next_dist_steps(footstep_nodes_list[step_index+1 >= step_num ? step_index : step_index+1]); + if (step_index > 0 && walking_phase == DOUBLE_BEFORE) { + next_dist_steps = dist_steps; + dist_steps = cur_steps; + if (step_index > 1) cur_steps = footstep_nodes_list[step_index-2]; + } + std::vector > ref_zmp_traj; + hrp::Vector3 cur_pos(hrp::Vector3::Zero()), next_pos(hrp::Vector3::Zero()), next2_pos(hrp::Vector3::Zero()), mid_pos(hrp::Vector3::Zero()), mid2_pos(hrp::Vector3::Zero()); + for (std::vector::iterator it = cur_steps.begin(); it != cur_steps.end(); it++) { + cur_pos += it->worldcoords.pos + it->worldcoords.rot * rg.get_default_zmp_offset(it->l_r); + } + cur_pos /= cur_steps.size(); + for (std::vector::iterator it = dist_steps.begin(); it != dist_steps.end(); it++) { + next_pos += it->worldcoords.pos + it->worldcoords.rot * rg.get_default_zmp_offset(it->l_r); + } + next_pos /= dist_steps.size(); + for (std::vector::iterator it = next_dist_steps.begin(); it != next_dist_steps.end(); it++) { + next2_pos += it->worldcoords.pos + it->worldcoords.rot * rg.get_default_zmp_offset(it->l_r); + } + next2_pos /= next_dist_steps.size(); + if (step_num_phase == FIRST || + (step_num_phase == AFTER_FIRST && walking_phase == DOUBLE_BEFORE)) { + hrp::Vector3 zmp_off = 0.5 * (rg.get_default_zmp_offset(RLEG) + rg.get_default_zmp_offset(LLEG)); + cur_pos = initial_foot_mid_coords.pos + initial_foot_mid_coords.rot * zmp_off; + mid_pos = cur_pos; + } else { + mid_pos = 0.5 * (cur_pos + next_pos); + } + bool is_after_last2 = false; + if ((step_num_phase == BEFORE_LAST && walking_phase != DOUBLE_BEFORE) || + step_num_phase == LAST) { + next_pos = mid_pos; + is_after_last2 = true; + } + mid2_pos = 0.5 * (next_pos + next2_pos); + if ((step_num_phase == BEFORE2_LAST && walking_phase != DOUBLE_BEFORE) || + step_num_phase == BEFORE_LAST || step_num_phase == LAST) { + if (is_after_last2) next2_pos = mid_pos; + else { + hrp::Vector3 zmp_off = 0.5 * (rg.get_default_zmp_offset(RLEG) + rg.get_default_zmp_offset(LLEG)); + hrp::Vector3 last_foot_pos = footstep_nodes_list[step_num - 2].front().worldcoords.pos + footstep_nodes_list[step_num - 2].front().worldcoords.rot * zmp_off; + next2_pos = 0.5 * (next_pos + last_foot_pos); + } + mid2_pos = next2_pos; + } + if (step_num_phase == LAST) { + cur_pos = mid_pos; + } + coordinates cur_step = coordinates(cur_pos, cur_steps.front().worldcoords.rot); + hrp::Vector3 last_cp = next_pos; + + // calc touchoff_remain_count for abc and st + const leg_type& cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r; + size_t next_step_count = static_cast(footstep_nodes_list[step_index + (step_num_phase == LAST ? 0 : 1)].front().step_time/dt); + touchoff_remain_count[0] = touchoff_remain_count[1] = remain_count; + if ((step_num_phase == BEFORE2_LAST && walking_phase != DOUBLE_BEFORE) || + (step_num_phase == BEFORE_LAST && walking_phase == DOUBLE_BEFORE)) { + next_step_count -= static_cast(next_step_count * default_double_support_ratio_before); + } + for (size_t i = 0; i < 2; i++) { + if (step_num_phase != LAST && + (cur_leg != i && walking_phase == DOUBLE_BEFORE || // assume rleg, lleg are 0, 1 + (cur_leg == i && walking_phase != DOUBLE_BEFORE)) + ) { + touchoff_remain_count[i] += next_step_count + 1; + } + } + + // to interpolate for cog after walking + if (use_act_states && + num_preview_step == 1 && step_num_phase == LAST && + walking_phase != DOUBLE_BEFORE) { + remain_count = std::max(static_cast(remain_count), static_cast(footguided_balance_time_const/dt)); + traj_remain_count = std::max(static_cast(remain_count), static_cast(footguided_balance_time_const/dt)); + } + + if (num_preview_step >= 1) { + ref_zmp_traj.reserve(5); + // cp y offset + if (num_preview_step == 1) calc_last_cp(last_cp, cur_step); + if (walking_phase == DOUBLE_BEFORE && + (step_num_phase == FIRST || step_num_phase == LAST)) { + ref_zmp_traj.push_back(LinearTrajectory(cur_ref_zmp, cur_ref_zmp, traj_remain_count*dt)); + ref_zmp_traj.push_back(LinearTrajectory(cur_ref_zmp, cur_pos, (footstep_nodes_list[step_index].front().step_time * (1.0 - default_double_support_ratio_before - default_double_support_ratio_after)))); + ref_zmp_traj.push_back(LinearTrajectory(cur_pos, mid_pos, (footstep_nodes_list[step_index].front().step_time * default_double_support_ratio_after))); + if (step_num_phase != LAST) ref_zmp_traj.push_back(LinearTrajectory(mid_pos, next_pos, (footstep_nodes_list[step_index + 1].front().step_time * default_double_support_ratio_before))); + } else if (walking_phase == DOUBLE_BEFORE) { + ref_zmp_traj.push_back(LinearTrajectory(cur_ref_zmp, next_pos, traj_remain_count*dt)); + } else if (walking_phase == DOUBLE_AFTER) { + ref_zmp_traj.push_back(LinearTrajectory(cur_ref_zmp, mid_pos, traj_remain_count*dt)); + if (step_num_phase != BEFORE_LAST && step_num_phase != LAST) ref_zmp_traj.push_back(LinearTrajectory(mid_pos, next_pos, (footstep_nodes_list[step_index + (step_num_phase == LAST ? 0 : 1)].front().step_time * default_double_support_ratio_before) + dt)); + } else { + ref_zmp_traj.push_back(LinearTrajectory(cur_ref_zmp, cur_pos, traj_remain_count*dt)); + ref_zmp_traj.push_back(LinearTrajectory(cur_pos, mid_pos, (footstep_nodes_list[step_index].front().step_time * default_double_support_ratio_after))); + if (step_num_phase != BEFORE_LAST && step_num_phase != LAST) ref_zmp_traj.push_back(LinearTrajectory(mid_pos, next_pos, (footstep_nodes_list[step_index + (step_num_phase == LAST ? 0 : 1)].front().step_time * default_double_support_ratio_before + dt))); + } + } + if (num_preview_step >= 2) { + last_cp = next2_pos; + bool not_last_second = (!(step_num_phase == BEFORE2_LAST && walking_phase != DOUBLE_BEFORE) && + !(step_num_phase == BEFORE_LAST && walking_phase == DOUBLE_BEFORE)); + if (not_last_second) calc_last_cp(last_cp, cur_step); + ref_zmp_traj.push_back(LinearTrajectory(next_pos, next_pos, (footstep_nodes_list[step_index + (step_num_phase == LAST ? 0 : 1)].front().step_time * (1.0 - default_double_support_ratio_before - default_double_support_ratio_after)))); + ref_zmp_traj.push_back(LinearTrajectory(next_pos, mid2_pos, (footstep_nodes_list[step_index + (step_num_phase == LAST ? 0 : 1)].front().step_time * default_double_support_ratio_after))); + if (not_last_second) ref_zmp_traj.push_back(LinearTrajectory(mid2_pos, next2_pos, (footstep_nodes_list[step_index + (step_num_phase == LAST ? 0 : (step_num_phase == BEFORE_LAST ? 1 : 2))].front().step_time * default_double_support_ratio_before))); + } + if (num_preview_step >= 3) { + std::cerr << "num_preview_step >= 3 is not supported" << std::endl; + solved = false; + } + + if (solved && is_wheeling && update_wheel_controller()) { + double w_sum_time = 0.0, f_sum_time = 0.0; + int f_idx = 0; + double traj_remain_time = wheel_interpolator->get_remain_time(); + hrp::Vector3 zmp_off = 0.5 * (rg.get_default_zmp_offset(RLEG) + rg.get_default_zmp_offset(LLEG)); + int cur_wlist_size = wheel_nodes_list.at(wheel_major_index).size(); + std::vector > tmp_zmp_traj = ref_zmp_traj; + const hrp::Vector3 initial = initial_wheel_midcoords.pos + initial_wheel_midcoords.rot * zmp_off; + final_footstep_pos = ref_zmp_traj.front().getStart() - initial; + ref_zmp_traj.clear(); + ref_zmp_traj.reserve(ref_zmp_traj.size() + cur_wlist_size); + + hrp::Vector3 goal; + for (int i = wheel_index; i < wheel_nodes_list.at(wheel_major_index).size() - 1; i++) { + const hrp::Vector3 start = (i == wheel_index ? + wheel_midcoords.pos + wheel_midcoords.rot * zmp_off : + (wheel_nodes_list.at(wheel_major_index).at(i).worldcoords.pos + wheel_nodes_list.at(wheel_major_index).at(i).worldcoords.rot * zmp_off)) - initial; + goal = wheel_nodes_list.at(wheel_major_index).at(i + 1).worldcoords.pos + wheel_nodes_list.at(wheel_major_index).at(i + 1).worldcoords.rot * zmp_off - initial; + const double duration = (i == wheel_index ? + traj_remain_time : wheel_nodes_list.at(wheel_major_index).at(i + 1).time); + while (f_idx < tmp_zmp_traj.size()) { + const hrp::Vector3 f_start = tmp_zmp_traj.at(f_idx).getStart(); + const hrp::Vector3 f_goal = tmp_zmp_traj.at(f_idx).getGoal(); + const double f_duration = tmp_zmp_traj.at(f_idx).getTime(); + ref_zmp_traj.push_back(LinearTrajectory(f_start, f_goal, f_duration, f_sum_time) + LinearTrajectory(start, goal, duration, w_sum_time)); // combine two lines + if (f_sum_time + f_duration >= w_sum_time + duration) break; + f_sum_time += f_duration; + f_idx++; + } + if (f_idx == tmp_zmp_traj.size()) break; + w_sum_time += duration; + } + last_cp += goal; + cur_ref_zmp = ref_zmp_traj.front().getStart(); + } + + ref_zmp_traj.push_back(LinearTrajectory(last_cp, last_cp, 1)); + + hrp::Vector3 feedforward_zmp; + foot_guided_controller_ptr->update_control(zmp, feedforward_zmp, ref_zmp_traj); + + if (num_preview_step == 1 && + (walking_phase == DOUBLE_BEFORE || walking_phase == DOUBLE_AFTER)) { + double tmp_ratio = 0.0; + if (double_support_zmp_interpolator->isEmpty()) { + double tmp_time = remain_count * dt; + double_support_zmp_interpolator->set(&tmp_ratio); + tmp_ratio = 1.0; + double_support_zmp_interpolator->setGoal(&tmp_ratio, tmp_time, true); + } + double_support_zmp_interpolator->get(&tmp_ratio, true); + hrp::Vector3 tmp_zmp = (use_act_states && is_interpolate_zmp_in_double) ? cur_ref_zmp : feedforward_zmp; + zmp = tmp_ratio * tmp_zmp + (1.0 - tmp_ratio) * zmp; + } else if (!double_support_zmp_interpolator->isEmpty()) { + double_support_zmp_interpolator->clear(); + } + + hrp::Vector3 refzmp_orig = zmp, feedforward_zmp_orig = feedforward_zmp; + { + // project to nominal ground + project_to_nominal_ground(zmp, cur_ref_zmp, cur_cog); + project_to_nominal_ground(feedforward_zmp, cur_ref_zmp, cur_refcog); + // truncate zmp (assume RLEG, LLEG) + Eigen::Vector2d tmp_zmp(zmp.head(2)), tmp_fzmp(feedforward_zmp.head(2)); + if (!is_inside_convex_hull(tmp_zmp, hrp::Vector3::Zero(), true)) { // TODO: should consider footstep rot + zmp.head(2) = tmp_zmp; + } + if (!is_inside_convex_hull(tmp_fzmp, hrp::Vector3::Zero(), true)) { // TODO: should consider footstep rot + feedforward_zmp.head(2) = tmp_fzmp; + } + // revert to zmp height + project_to_nominal_ground(zmp, refzmp_orig, cur_cog); + project_to_nominal_ground(feedforward_zmp, feedforward_zmp_orig, cur_refcog); + // zmp = zmp_filter->passFilter(zmp); + foot_guided_controller_ptr->set_zmp(zmp, feedforward_zmp); + } + // calc cog + hrp::Vector3 tmpfxy = hrp::Vector3::Zero(); + if (use_disturbance_compensation) tmpfxy = fxy; + foot_guided_controller_ptr->update_state(cog, tmpfxy); + // convert zmp -> refzmp + refzmp = zmp; + project_to_nominal_ground(refzmp, cur_ref_zmp, cur_cog); + project_to_nominal_ground(feedforward_zmp, cur_ref_zmp, cur_refcog); + + // for log + tmp[0] = cur_ref_zmp(0); + tmp[1] = cur_ref_zmp(1); + tmp[21] = feedforward_zmp(0); + tmp[22] = feedforward_zmp(1); + tmp[2] = last_cp(0); + tmp[3] = last_cp(1); + tmp[4] = refzmp(0); + tmp[5] = refzmp(1); + tmp[6] = ref_cp(0); + tmp[7] = ref_cp(1); + tmp[8] = act_cp(0); + tmp[9] = act_cp(1); + tmp[10] = traj_remain_count*dt; + tmp[11] = remain_count*dt; + tmp[12] = flywheel_tau(0); + tmp[13] = flywheel_tau(1); + tmp[14] = falling_direction; + } + + void gait_generator::calc_last_cp(hrp::Vector3& last_cp, const coordinates& cur_step) + { + if (!(step_num_phase == BEFORE_LAST && walking_phase != DOUBLE_BEFORE) && + step_num_phase != LAST) { + hrp::Matrix33 cur_step_rot; + calc_foot_origin_rot(cur_step_rot, cur_step.rot); + last_cp = cur_step_rot.transpose() * (last_cp - cur_step.pos); + last_cp(1) += (last_cp(1) > 0 ? -1 : 1) * dcm_offset; + last_cp = cur_step.pos + cur_step_rot * last_cp; + } + } + + void gait_generator::set_first_count_flag () + { + if (is_first_count) is_first_count = false; + if (remain_count == 1 || is_set_first_count) is_first_count = true; + } + + void gait_generator::limit_stride (step_node& cur_fs, const step_node& prev_fs, const double (&limit)[5]) const + { + // limit[5] = {forward, outside, theta, backward, inside} + leg_type cur_leg = cur_fs.l_r; + // prev_fs frame + hrp::Matrix33 prev_fs_rot; + calc_foot_origin_rot(prev_fs_rot, prev_fs.worldcoords.rot); + cur_fs.worldcoords.pos = prev_fs_rot.transpose() * (cur_fs.worldcoords.pos - prev_fs.worldcoords.pos); + double stride_r = std::pow(cur_fs.worldcoords.pos(0), 2.0) + std::pow(cur_fs.worldcoords.pos(1) + footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) - footstep_param.leg_default_translate_pos[cur_leg](1), 2.0); + // front, rear, outside limitation + double stride_r_limit = std::pow(std::max(limit[cur_fs.worldcoords.pos(0) >= 0 ? 0 : 3], limit[1] - limit[4]), 2.0); + if (stride_r > stride_r_limit) { + cur_fs.worldcoords.pos(0) *= sqrt(stride_r_limit / stride_r); + cur_fs.worldcoords.pos(1) = footstep_param.leg_default_translate_pos[cur_leg](1) - footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) + + sqrt(stride_r_limit / stride_r) * (cur_fs.worldcoords.pos(1) + footstep_param.leg_default_translate_pos[cur_leg == LLEG ? RLEG : LLEG](1) - footstep_param.leg_default_translate_pos[cur_leg](1)); + } + if (cur_fs.worldcoords.pos(0) > limit[0]) cur_fs.worldcoords.pos(0) = limit[0]; + if (cur_fs.worldcoords.pos(0) < -1 * limit[3]) cur_fs.worldcoords.pos(0) = -1 * limit[3]; + if ((cur_leg == LLEG ? 1 : -1) * cur_fs.worldcoords.pos(1) > limit[1]) cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * limit[1]; + // inside limitation + std::vector cur_leg_vertices_y; + cur_leg_vertices_y.reserve(4); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs_rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1)); + if (cur_leg == LLEG) { + if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < limit[4]) cur_fs.worldcoords.pos(1) += limit[4] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()); + } else { + if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * limit[4]) cur_fs.worldcoords.pos(1) += -1 * limit[4] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()); + } + // world frame + cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs_rot * cur_fs.worldcoords.pos; + }; + + void gait_generator::limit_stride_rectangle (step_node& cur_fs, const step_node& prev_fs, const double (&limit)[5]) const + { + // limit[5] = {forward, outside, theta, backward, inside} + leg_type cur_leg = cur_fs.l_r; + // prev_fs frame + hrp::Matrix33 prev_fs_rot; + calc_foot_origin_rot(prev_fs_rot, prev_fs.worldcoords.rot); + cur_fs.worldcoords.pos = prev_fs_rot.transpose() * (cur_fs.worldcoords.pos - prev_fs.worldcoords.pos); + // front, rear, outside limitation + if (cur_fs.worldcoords.pos(0) > limit[0]) cur_fs.worldcoords.pos(0) = limit[0]; + if (cur_fs.worldcoords.pos(0) < -1 * limit[0]) cur_fs.worldcoords.pos(0) = -1 * limit[3]; + if ((cur_leg == LLEG ? 1 : -1) * cur_fs.worldcoords.pos(1) > limit[1]) cur_fs.worldcoords.pos(1) = (cur_leg == LLEG ? 1 : -1) * limit[1]; + // inside limitation + std::vector cur_leg_vertices_y; + cur_leg_vertices_y.reserve(4); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(leg_margin[0], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? 1 : -1) * leg_margin[2], 0.0))(1)); + cur_leg_vertices_y.push_back((cur_fs.worldcoords.pos + prev_fs.worldcoords.rot.transpose() * cur_fs.worldcoords.rot * hrp::Vector3(-1 * leg_margin[1], (cur_leg == LLEG ? -1 : 1) * leg_margin[3], 0.0))(1)); + if (cur_leg == LLEG) { + if (*std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) < limit[4]) cur_fs.worldcoords.pos(1) += limit[4] - *std::min_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()); + } else { + if (*std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()) > -1 * limit[4]) cur_fs.worldcoords.pos(1) += -1 * limit[4] - *std::max_element(cur_leg_vertices_y.begin(), cur_leg_vertices_y.end()); + } + // world frame + cur_fs.worldcoords.pos = prev_fs.worldcoords.pos + prev_fs_rot * cur_fs.worldcoords.pos; + }; + + void gait_generator::limit_stride_vision (step_node& cur_fs, hrp::Vector3& short_of_footstep, const step_node& prev_fs, const step_node& preprev_fs, const double& omega, const hrp::Vector3& cur_cp) + { + // preprev foot frame => + hrp::Matrix33 preprev_fs_rot, prev_fs_rot; + hrp::Vector3 prev_fs_pos, preprev_fs_pos = preprev_fs.worldcoords.pos; + calc_foot_origin_rot(preprev_fs_rot, preprev_fs.worldcoords.rot); + prev_fs_pos = preprev_fs_rot.transpose() * (prev_fs.worldcoords.pos - preprev_fs_pos); + // <= preprev foot frame + + hrp::Vector3 fspos_for_touch_wall = cur_fs.worldcoords.pos; + + // preprev foot frame + cur_fs.worldcoords.pos = preprev_fs_rot.transpose() * (cur_fs.worldcoords.pos - preprev_fs_pos); + calc_foot_origin_rot(prev_fs_rot, prev_fs.worldcoords.rot); + prev_fs_rot = preprev_fs_rot.transpose() * prev_fs_rot; + short_of_footstep = hrp::Vector3::Zero(); + hrp::Vector3 rel_cur_cp = preprev_fs_rot.transpose() * (cur_cp - preprev_fs_pos); + leg_type cur_sup = prev_fs.l_r; + + double limit_r = std::min(safe_leg_margin[2], safe_leg_margin[3]), new_remain_time, tmp_dt = 1e10; + bool is_out = false, is_read_stepable = false; + Eigen::Vector2d tmp_pos = Eigen::Vector2d::Zero(), new_pos; + hrp::Vector3 tmp_short, new_short; + double tmp_height = cur_fs.worldcoords.pos(2); + for (size_t i = 0; i < steppable_region[cur_sup].size(); i++) { + if (steppable_region[cur_sup][i].size() < 3) continue; + is_read_stepable = true; + new_remain_time = remain_count * dt; + new_pos = cur_fs.worldcoords.pos.head(2); + new_short = hrp::Vector3::Zero(); + if (!is_inside_convex_hull(new_pos, new_remain_time, new_short, steppable_region[cur_sup][i], hrp::Vector3::Zero(), false, true, limit_r, omega, rel_cur_cp, prev_fs_pos, prev_fs_rot, cur_sup)) { + if (cur_fs.worldcoords.pos.head(2).dot(new_pos) > cur_fs.worldcoords.pos.head(2).dot(tmp_pos) || !is_out) { + is_out = true; + tmp_dt = new_remain_time - remain_count*dt; + tmp_pos = new_pos; + tmp_short = new_short; + tmp_height = steppable_height[cur_sup][i]; + } + } else { + is_out = false; + tmp_height = steppable_height[cur_sup][i]; + break; + } + } + if (is_out) { + if (cur_fs.worldcoords.pos(0) > tmp_pos(0)) { // when stride is limited by front edge of steppable region + cur_fs.worldcoords.pos.head(2) = tmp_pos + front_edge_offset_of_steppable_region; + } else { + cur_fs.worldcoords.pos.head(2) = tmp_pos; + } + change_step_time(tmp_dt); + short_of_footstep = tmp_short; + } + if (!was_read_steppable_height) { + was_read_steppable_height = true; + cur_fs.worldcoords.pos(2) = tmp_height; + debug_steppable_height = tmp_height; + } + + // world frame + cur_fs.worldcoords.pos = preprev_fs_pos + preprev_fs_rot * cur_fs.worldcoords.pos; + short_of_footstep = preprev_fs_rot * short_of_footstep; + if (!is_read_stepable) cur_fs.worldcoords.pos = preprev_fs_pos; + + // check for touch wall + // TODO: should consider the case where is_out = true while footstep is inside the limit_stride (like stepping stone) + if (short_of_footstep.norm() > 5e-2 && (cur_fs.worldcoords.pos - fspos_for_touch_wall).norm() > 1e-2 && lcg.get_footstep_index() > 1) { // 1cm + is_emergency_touch_wall = true; + } + } + + void gait_generator::modify_footsteps_for_recovery () + { + if (isfinite(diff_cp(0)) && isfinite(diff_cp(1))) { + // calculate diff_cp + hrp::Vector3 tmp_diff_cp; + for (size_t i = 0; i < 2; i++) { + if (std::fabs(diff_cp(i)) > cp_check_margin[i]) { + is_emergency_walking[i] = true; + tmp_diff_cp(i) = diff_cp(i) - cp_check_margin[i] * diff_cp(i)/std::fabs(diff_cp(i)); + } else { + is_emergency_walking[i] = false; + } + } + if (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2) { + // calculate sum of preview_f + static double preview_f_sum; + if (lcg.get_lcg_count() == static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1) { + preview_f_sum = preview_controller_ptr->get_preview_f(preview_controller_ptr->get_delay()); + for (size_t i = preview_controller_ptr->get_delay()-1; i >= lcg.get_lcg_count()+1; i--) { + preview_f_sum += preview_controller_ptr->get_preview_f(i); + } + modified_d_footstep = hrp::Vector3::Zero(); + } + if (lcg.get_lcg_count() <= preview_controller_ptr->get_delay()) { + preview_f_sum += preview_controller_ptr->get_preview_f(lcg.get_lcg_count()); + } + // calculate modified footstep position + double preview_db = 1/6.0 * dt * dt * dt + 1/2.0 * dt * dt * 1/std::sqrt(gravitational_acceleration / (cog(2) - refzmp(2))); + hrp::Vector3 d_footstep = -1/preview_f_sum * 1/preview_db * footstep_modification_gain * tmp_diff_cp; + d_footstep(2) = 0.0; + // overwrite footsteps + if (lcg.get_lcg_count() <= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1 && + lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (default_double_support_ratio_after + margin_time_ratio)) - 1 && + !(lcg.get_lcg_count() <= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1 && act_contact_states[0] && act_contact_states[1])) { + // stride limitation check + hrp::Vector3 orig_footstep_pos = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos; + for (size_t i = 0; i < 2; i++) { + if (is_emergency_walking[i]) footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos(i) += d_footstep(i); + } + limit_stride(footstep_nodes_list[get_overwritable_index()].front(), footstep_nodes_list[get_overwritable_index()-1].front(), overwritable_stride_limitation); + d_footstep = footstep_nodes_list[get_overwritable_index()].front().worldcoords.pos - orig_footstep_pos; + for (size_t i = lcg.get_footstep_index()+1; i < footstep_nodes_list.size(); i++) { + footstep_nodes_list[i].front().worldcoords.pos += d_footstep; + } + if (is_emergency_walking[0] || is_emergency_walking[1]) { + overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+lcg.get_footstep_index(), footstep_nodes_list.end()); + // overwrite zmp + overwrite_refzmp_queue(overwrite_footstep_nodes_list); + overwrite_footstep_nodes_list.clear(); + modified_d_footstep += d_footstep; + } + } + } else { + modified_d_footstep = hrp::Vector3::Zero(); + } + } + } + + void gait_generator::modify_footsteps_for_foot_guided (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_cogvel, const hrp::Vector3& cur_refcog, const hrp::Vector3& cur_refcogvel, const hrp::Vector3& target_cog) + { + double omega = std::sqrt((gravitational_acceleration - total_external_force_z/total_mass) / foot_guided_controller_ptr->get_dz()); + bool is_modify = false; + hrp::Vector3 orig_footstep_pos = footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos; + hrp::Matrix33 orig_footstep_rot = footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.rot; + hrp::Vector3 d_footstep = hrp::Vector3::Zero(), short_of_footstep = hrp::Vector3::Zero(); + hrp::Vector3 cur_footstep_pos = footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.pos; + hrp::Matrix33 cur_footstep_rot; + calc_foot_origin_rot(cur_footstep_rot, footstep_nodes_list[lcg.get_footstep_index()-1].front().worldcoords.rot); + hrp::Matrix33 next_footstep_rot; + calc_foot_origin_rot(next_footstep_rot, footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.rot); + leg_type cur_sup = footstep_nodes_list[lcg.get_footstep_index()-1].front().l_r; + hrp::Vector3 cur_cp = cp_filter->passFilter(cur_cog + cur_cogvel / omega); + cur_cp = cur_footstep_rot.transpose() * (cur_cp - cur_footstep_pos); + hrp::Vector3 next_step_pos = cur_footstep_rot.transpose() * (orig_footstep_pos - cur_footstep_pos); + bool is_modify_pahse = false; + double stride_limitation_with_off[5]; + // assume that zmp_offsts is set to be symmetry + next_step_pos(0) += rg.get_default_zmp_offset(RLEG)(0); + next_step_pos(1) += (rg.get_default_zmp_offset(RLEG)(1) + dcm_offset) * (cur_sup == RLEG ? -1 : 1); + stride_limitation_with_off[0] = overwritable_stride_limitation[0] + rg.get_default_zmp_offset(RLEG)(0); + stride_limitation_with_off[1] = overwritable_stride_limitation[1] - rg.get_default_zmp_offset(RLEG)(1) - dcm_offset; + stride_limitation_with_off[3] = overwritable_stride_limitation[3] - rg.get_default_zmp_offset(RLEG)(0); + stride_limitation_with_off[4] = overwritable_stride_limitation[4] - rg.get_default_zmp_offset(RLEG)(1) - dcm_offset; + // stride_limitation_with_off[0] = overwritable_stride_limitation[0]; + // stride_limitation_with_off[1] = overwritable_stride_limitation[1]; + // stride_limitation_with_off[3] = overwritable_stride_limitation[3]; + // stride_limitation_with_off[4] = overwritable_stride_limitation[4]; + + if (lcg.get_lcg_count() >= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (default_double_support_ratio_after + margin_time_ratio)) && + lcg.get_lcg_count() < static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * (1.0 - default_double_support_ratio_before)) - 1 && + // !(lcg.get_lcg_count() <= static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1 && act_contact_states[0] && act_contact_states[1]) + !(act_contact_states[0] && act_contact_states[1]) + ) { + is_modify_pahse = true; + // step timing extenstion for stair + if (is_slow_stair_mode && lcg.get_footstep_index() > 1) { // 1st step is not extended + double stair_margin = 0.02; + if (!changed_step_time_stair && + std::fabs(footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(2) - footstep_nodes_list[lcg.get_footstep_index()-2].front().worldcoords.pos(2)) > std::fabs(lcg.get_default_step_height() - stair_margin) && + default_step_time + stair_step_time > footstep_nodes_list[lcg.get_footstep_index()].front().step_time) { + double tmp_dt = default_step_time + stair_step_time - footstep_nodes_list[lcg.get_footstep_index()].front().step_time; + change_step_time(tmp_dt); + changed_step_time_stair = true; + } + } + // step timing modification + { + double tmp_dt = 0.0; + bool is_change_time = false; + double new_remain_time = remain_count * dt; + { + double remain_time = remain_count * dt; + double end_cp_front = std::exp(omega * remain_time) * cur_cp(0) - (std::exp(omega * remain_time) - 1) * safe_leg_margin[0]; + double end_cp_back = std::exp(omega * remain_time) * cur_cp(0) + (std::exp(omega * remain_time) - 1) * safe_leg_margin[1]; + double end_cp_inside = std::exp(omega * remain_time) * cur_cp(1) - (cur_sup == RLEG ? 1 : -1) * (std::exp(omega * remain_time) - 1) * safe_leg_margin[3]; + double xz_max, l_max; + bool tmp_is_change_time = false; + // front, back + if (end_cp_front > stride_limitation_with_off[0]) { + tmp_is_change_time = true; + xz_max = safe_leg_margin[0]; + l_max = stride_limitation_with_off[0]; + } else if (end_cp_back < -1 * stride_limitation_with_off[3]) { + tmp_is_change_time = true; + xz_max = -1 * safe_leg_margin[1]; + l_max = -1 * stride_limitation_with_off[3]; + } + if (tmp_is_change_time) new_remain_time = std::min(new_remain_time, (std::log((l_max - xz_max) / (cur_cp(0) - xz_max)) / omega)); + // inside + if ((cur_sup == RLEG ? 1 : -1) * end_cp_inside > stride_limitation_with_off[1]) { + xz_max = (cur_sup == RLEG ? 1 : -1) * safe_leg_margin[3]; + l_max = (cur_sup == RLEG ? 1 : -1) * stride_limitation_with_off[1]; + new_remain_time = std::min(new_remain_time, (std::log((l_max - xz_max) / (cur_cp(1) - xz_max)) / omega)); + } + if (std::isfinite(new_remain_time)) tmp_dt = new_remain_time - remain_count*dt; + // min time check + if (tmp_dt < 0) { + min_time_check(tmp_dt); + is_change_time = true; + } else { + tmp_dt = 0.0; + } + } + // outside check + { + double inside_off = stride_limitation_with_off[4] + leg_margin[3]; + double tmp_remain_time = remain_count * dt + tmp_dt; + double inside_cp = std::exp(omega * tmp_remain_time) * cur_cp(1) - (cur_sup == RLEG ? -1 : 1) * (std::exp(omega * tmp_remain_time) - 1) * safe_leg_margin[2]; + if ((cur_sup == RLEG ? 1 : -1) * inside_cp < inside_off && safe_leg_margin[2] > (cur_sup == RLEG ? -1 : 1) * cur_cp(1)) { + new_remain_time = std::log((inside_off + safe_leg_margin[2]) / ((cur_sup == RLEG ? 1 : -1) * cur_cp(1) + safe_leg_margin[2])) / omega; + if (std::isfinite(new_remain_time)) { + is_change_time = true; + was_enlarged_time = true; + tmp_dt = new_remain_time - remain_count*dt; + if (footstep_nodes_list[lcg.get_footstep_index()].front().step_time + tmp_dt > overwritable_max_time) { + tmp_dt = overwritable_max_time - footstep_nodes_list[lcg.get_footstep_index()].front().step_time; + } + } + } else if (safe_leg_margin[2] <= (cur_sup == RLEG ? -1 : 1) * cur_cp(1)) { + tmp_dt = -0.001; // change to min_time + min_time_check(tmp_dt); + is_change_time = true; + } else if (was_enlarged_time) { + new_remain_time = std::log((inside_off + safe_leg_margin[2]) / ((cur_sup == RLEG ? 1 : -1) * cur_cp(1) + safe_leg_margin[2])) / omega; + if (std::isfinite(new_remain_time)) { + tmp_dt = new_remain_time - remain_count*dt; + if (footstep_nodes_list[lcg.get_footstep_index()].front().step_time + tmp_dt > overwritable_max_time) { + tmp_dt = overwritable_max_time - footstep_nodes_list[lcg.get_footstep_index()].front().step_time; + } + } + min_time_check(tmp_dt); + is_change_time = true; + } + } + if (is_change_time) { + change_step_time(tmp_dt); + } + } + // footstep modification + { + double remain_time = remain_count * dt; + hrp::Vector3 tmp_zmp = - (next_step_pos - std::exp(omega * remain_time) * cur_cp) / (std::exp(omega * remain_time) - 1); + if (tmp_zmp(0) > safe_leg_margin[0]) { // front + d_footstep(0) = std::exp(omega * remain_time) * cur_cp(0) - (std::exp(omega * remain_time) - 1) * safe_leg_margin[0] - next_step_pos(0); + is_modify = true; + } else if (tmp_zmp(0) < - safe_leg_margin[1]) { // rear + d_footstep(0) = std::exp(omega * remain_time) * cur_cp(0) - (std::exp(omega * remain_time) - 1) * -1 * safe_leg_margin[1] - next_step_pos(0); + is_modify = true; + } else if ((cur_sup == LLEG ? 1 : -1) * tmp_zmp(1) > safe_leg_margin[2]) { // outside + d_footstep(1) = std::exp(omega * remain_time) * cur_cp(1) - (std::exp(omega * remain_time) - 1) * (cur_sup == LLEG ? 1 : -1) * safe_leg_margin[2] - next_step_pos(1); + is_modify = true; + } else if ((cur_sup == LLEG ? -1 : 1) * tmp_zmp(1) > safe_leg_margin[3]) { // inside + d_footstep(1) = std::exp(omega * remain_time) * cur_cp(1) - (std::exp(omega * remain_time) - 1) * (cur_sup == LLEG ? -1 : 1) * safe_leg_margin[3] - next_step_pos(1); + is_modify = true; + } + // emergency step when standing + if (is_emergency_step && footstep_nodes_list.size() - lcg.get_footstep_index() == 3 && is_modify + && d_footstep.norm() > 1e-2 + ) { + leg_type cur_leg = footstep_nodes_list[lcg.get_footstep_index()].front().l_r; + footstep_nodes_list.push_back(boost::assign::list_of(step_node(cur_leg==RLEG?LLEG:RLEG, footstep_nodes_list[lcg.get_footstep_index()+1].front().worldcoords, lcg.get_default_step_height(), default_step_time, 0, 0))); + } + d_footstep = cur_footstep_rot * d_footstep; // foot coords -> world coords + d_footstep(2) = 0.0; + if (is_modify || is_vision_updated || lr_region[cur_sup] || debug_set_landing_height) { + step_node preprev_fs = (lcg.get_footstep_index()==1 ? lcg.get_swing_leg_src_steps().front() : footstep_nodes_list[lcg.get_footstep_index()-2].front()); + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos += d_footstep; + short_of_footstep = d_footstep; + if (lr_region[cur_sup]) { + limit_stride_vision(footstep_nodes_list[lcg.get_footstep_index()].front(), short_of_footstep, footstep_nodes_list[lcg.get_footstep_index()-1].front(), preprev_fs, omega, cur_footstep_pos + cur_footstep_rot * cur_cp); + } else { + limit_stride_rectangle(footstep_nodes_list[lcg.get_footstep_index()].front(), footstep_nodes_list[lcg.get_footstep_index()-1].front(), overwritable_stride_limitation); + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(2) = orig_footstep_pos(2); + } + { + double tmp_height = (cur_footstep_pos + rel_landing_height)(2); + if ((is_vision_updated || debug_set_landing_height)) { + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(2) = tmp_height; + calc_foot_origin_rot(footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.rot, orig_footstep_rot, cur_footstep_rot * rel_landing_normal); + if (debug_set_landing_height) { + if (footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(0) > debug_landing_height_xrange[0] && footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(0) < debug_landing_height_xrange[1]) { + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(2) = debug_orig_height + debug_landing_height; + } else { + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(2) = debug_orig_height; + } + } + // TODO : why is yaw angle changed + hrp::Vector3 tmp_rpy = hrp::rpyFromRot(footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.rot); + tmp_rpy(2) = hrp::rpyFromRot(orig_footstep_rot)(2); + footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.rot = hrp::rotFromRpy(tmp_rpy); + } + } + d_footstep = footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos - orig_footstep_pos; + if (!(lr_region[cur_sup])) short_of_footstep = d_footstep - short_of_footstep; + for (size_t i = lcg.get_footstep_index()+1; i < footstep_nodes_list.size(); i++) { + footstep_nodes_list[i].front().worldcoords.pos += d_footstep; + } + } + overwrite_footstep_nodes_list.insert(overwrite_footstep_nodes_list.end(), footstep_nodes_list.begin()+lcg.get_footstep_index(), footstep_nodes_list.end()); + // overwrite zmp + overwrite_refzmp_queue(overwrite_footstep_nodes_list, cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); + overwrite_footstep_nodes_list.clear(); + modified_d_footstep += d_footstep; + sum_d_footstep_plus += d_footstep; + sum_d_footstep_minus += d_footstep; + } + } + // calculate angular momentum + use_roll_flywheel = use_pitch_flywheel = false; + { + hrp::Vector3 short_of_zmp = hrp::Vector3::Zero(); + double remain_time = remain_count * dt; + short_of_footstep = cur_footstep_rot.transpose() * short_of_footstep; + if (is_modify_pahse) { + for (size_t i = 0; i < 2; i++) { + if(std::fabs(short_of_footstep(i)) > 1e-3) { // > 1mm + if (use_flywheel_balance) { + if (i == 0) use_pitch_flywheel = true; + else use_roll_flywheel = true; + } + // short_of_zmp(i) = -short_of_footstep(i) / (std::exp(omega * (remain_time - dt)) * omega * dt); + short_of_zmp(i) = short_of_footstep(i) / (1 - std::exp(omega * remain_time)); + } + } + prev_short_of_zmp = short_of_zmp; + prev_use_roll_flywheel = use_roll_flywheel; + prev_use_pitch_flywheel = use_pitch_flywheel; + } else { + short_of_zmp = prev_short_of_zmp; + use_roll_flywheel = prev_use_roll_flywheel; + use_pitch_flywheel = prev_use_pitch_flywheel; + } + flywheel_tau = total_mass * (gravitational_acceleration - total_external_force_z/total_mass) * hrp::Vector3(-short_of_zmp(1), short_of_zmp(0), 0); + // if (use_roll_flywheel || use_pitch_flywheel) std::cerr << "torque :" << flywheel_tau.transpose()<< std::endl; + } + // fall down check // TODO: turn walking is not considered + { + // next step pos relative + double remain_time = remain_count * dt; + double end_cp_front = std::exp(omega * remain_time) * cur_cp(0) - (std::exp(omega * remain_time) - 1) * leg_margin[0]; + double end_cp_back = std::exp(omega * remain_time) * cur_cp(0) + (std::exp(omega * remain_time) - 1) * leg_margin[1]; + double end_cp_outside = std::exp(omega * remain_time) * cur_cp(1) - (cur_sup == RLEG ? -1 : 1) * (std::exp(omega * remain_time) - 1) * leg_margin[2]; + double end_cp_inside = std::exp(omega * remain_time) * cur_cp(1) - (cur_sup == RLEG ? 1 : -1) * (std::exp(omega * remain_time) - 1) * leg_margin[3]; + hrp::Vector3 new_footstep_pos = footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos; + hrp::Matrix33 new_footstep_rot; + calc_foot_origin_rot(new_footstep_rot, footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.rot); + hrp::Vector3 end_cp = new_footstep_pos; + size_t tmp_falling_direction = 0; + falling_direction = 0; + // x axis + double xz_max, l_max; + if (end_cp_front > overwritable_stride_limitation[0] + leg_margin[0]) { // front + tmp_falling_direction = 1; + xz_max = leg_margin[0]; + l_max = overwritable_stride_limitation[0]; + end_cp(0) = end_cp_front; + } else if (end_cp_back < -1 * (overwritable_stride_limitation[3] + leg_margin[1])) { // rear + tmp_falling_direction = 2; + xz_max = leg_margin[1]; + l_max = overwritable_stride_limitation[3]; + end_cp(0) = end_cp_back; + } + end_cp = new_footstep_rot.transpose() * ((cur_footstep_pos + cur_footstep_rot * end_cp) - new_footstep_pos); + if (tmp_falling_direction == 1 || tmp_falling_direction == 2) { + if ((tmp_falling_direction == 1 ? 1 : -1) * end_cp(0) > xz_max - l_max / (1 - std::exp(omega * min_time))) { + falling_direction = tmp_falling_direction; + } + } + // y axis + end_cp = new_footstep_pos; + double yz_1, yz_2, l_1, l_2; + if ((cur_sup == RLEG ? 1 : -1) * end_cp_inside > overwritable_stride_limitation[1] + leg_margin[2]) { // inside + tmp_falling_direction = 3; + yz_1 = leg_margin[2]; + yz_2 = leg_margin[3]; + l_1 = overwritable_stride_limitation[4] + leg_margin[3]; + l_2 = -1 * overwritable_stride_limitation[1]; + end_cp(1) = end_cp_inside; + } else if ((cur_sup == RLEG ? -1 : 1) * end_cp_outside > leg_margin[2]) { // outside + tmp_falling_direction = 4; + yz_1 = leg_margin[3]; + yz_1 = leg_margin[2]; + l_1 = -1 * overwritable_stride_limitation[1]; + l_2 = overwritable_stride_limitation[4] + leg_margin[3]; + end_cp(1) = end_cp_outside; + } + end_cp = new_footstep_rot.transpose() * ((cur_footstep_pos + cur_footstep_rot * end_cp) - new_footstep_pos); + if (tmp_falling_direction == 3 || tmp_falling_direction == 4) { + if(std::fabs(end_cp(1)) > (yz_1 + yz_2 * std::exp(omega * min_time)) / (1 + std::exp(omega * min_time)) + (l_1 + l_2 * std::exp(omega * min_time)) / (1 - std::exp(2 * omega * min_time))) { + falling_direction = tmp_falling_direction; + } + } + } + } + + void gait_generator::min_time_check (double& tmp_dt) + { + if (footstep_nodes_list[lcg.get_footstep_index()].front().step_time + tmp_dt < min_time) { + tmp_dt = min_time - footstep_nodes_list[lcg.get_footstep_index()].front().step_time; + } + if (remain_count*dt + tmp_dt < min_time_mgn) { + if (remain_count*dt < min_time_mgn) { + tmp_dt = 0.0; + } else { + tmp_dt = min_time_mgn - remain_count*dt; + } + } + } + + void gait_generator::change_step_time (const double& tmp_dt) + { + remain_count += tmp_dt / dt; + footstep_nodes_list[lcg.get_footstep_index()].front().step_time += tmp_dt; + size_t orig_lcg_cnt = lcg.get_lcg_count(); + lcg.set_lcg_count(lcg.get_lcg_count() + static_cast(tmp_dt/dt)); + lcg.set_one_step_count(lcg.get_lcg_count() - orig_lcg_cnt); + rg.set_one_step_count(lcg.get_lcg_count() - orig_lcg_cnt); + rg.set_refzmp_count(lcg.get_lcg_count()); + lcg.reset_one_step_count(lcg.get_lcg_count() - orig_lcg_cnt, default_double_support_ratio_before, default_double_support_ratio_after); + modified_d_step_time += tmp_dt; + } + + bool gait_generator::is_intersect (Eigen::Vector2d& r, const Eigen::Vector2d& a0, const Eigen::Vector2d& a1, const Eigen::Vector2d& b0, const Eigen::Vector2d& b1) + { + double D = calcCrossProduct(a1 - a0, b1 - b0); + if (D == 0.0) return false; + double t = calcCrossProduct(b0 - a0, b1 - b0) / D; + double s = -calcCrossProduct(a0 - b0, a1 - a0) / D; + r = a0 + t * (a1 - a0); + return (t >= 0.0 && t <= 1.0 && s >= 0.0 && s <= 1.0); + } + + std::vector gait_generator::calc_intersect_convex (std::vector& P, std::vector& Q) + { + const int n = P.size(), m = Q.size(); + int a = 0, b = 0, aa = 0, ba = 0; + enum { Pin, Qin, Unknown } in = Unknown; + std::vector R; + do { + int a1 = (a+n-1) % n, b1 = (b+m-1) % m; + double C = calcCrossProduct(P[a] - P[a1], Q[b] - Q[b1]); + double A = calcCrossProduct(P[a1] - Q[b], P[a] - Q[b]); + double B = calcCrossProduct(Q[b1] - P[a], Q[b] - P[a]); + Eigen::Vector2d r; + if (is_intersect(r, P[a1], P[a], Q[b1], Q[b])) { + if (in == Unknown) aa = ba = 0; + R.push_back(r); + in = B > 0 ? Pin : A > 0 ? Qin : in; + } + if (C == 0 && B == 0 && A == 0) { + if (in == Pin) { b = (b + 1) % m; ++ba; } + else { a = (a + 1) % m; ++aa; } + } else if (C >= 0) { + if (A > 0) { if (in == Pin) R.push_back(P[a]); a = (a+1)%n; ++aa; } + else { if (in == Qin) R.push_back(Q[b]); b = (b+1)%m; ++ba; } + } else { + if (B > 0) { if (in == Qin) R.push_back(Q[b]); b = (b+1)%m; ++ba; } + else { if (in == Pin) R.push_back(P[a]); a = (a+1)%n; ++aa; } + } + } while ( (aa < n || ba < m) && aa < 2*n && ba < 2*m ); + if (in == Unknown) { + if (is_inside_convex_hull(P[0], Q)) return P; + if (is_inside_convex_hull(Q[0], P)) return Q; + } + return R; + } + /* generate vector of step_node from :go-pos params * x, y and theta are simply divided by using stride params * unit system -> x [mm], y [mm], theta [deg] @@ -622,7 +2224,7 @@ namespace rats const std::vector& initial_support_legs_coords, coordinates start_ref_coords, const std::vector& initial_support_legs, std::vector< std::vector >& new_footstep_nodes_list, - const bool is_initialize) + const bool is_initialize, const double tm_off) { // Get overwrite footstep index size_t overwritable_fs_index = 0; @@ -637,7 +2239,7 @@ namespace rats if (overwritable_fs_index > footstep_nodes_list.size()-1) return false; go_pos_param_2_footstep_nodes_list_core (goal_x, goal_y, goal_theta, initial_support_legs_coords, start_ref_coords, initial_support_legs, - new_footstep_nodes_list, is_initialize, overwritable_fs_index); + new_footstep_nodes_list, is_initialize, overwritable_fs_index, tm_off); // For Last double support period if (is_initialize) { clear_footstep_nodes_list(); @@ -654,7 +2256,8 @@ namespace rats const std::vector& initial_support_legs_coords, coordinates start_ref_coords, const std::vector& initial_support_legs, std::vector< std::vector >& new_footstep_nodes_list, - const bool is_initialize, const size_t overwritable_fs_index) const + const bool is_initialize, const size_t overwritable_fs_index, + const double tm_off) const { // Calc goal ref coordinates goal_ref_coords; @@ -664,7 +2267,8 @@ namespace rats goal_ref_coords = initial_foot_mid_coords; step_node tmpfs = footstep_nodes_list[overwritable_fs_index-1].front(); start_ref_coords = tmpfs.worldcoords; - start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(-1*footstep_param.leg_default_translate_pos[tmpfs.l_r]); + // start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(-1*footstep_param.leg_default_translate_pos[tmpfs.l_r]); + start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(-1*footstep_param.vel_foot_offset[tmpfs.l_r]); } goal_ref_coords.pos += goal_ref_coords.rot * hrp::Vector3(goal_x, goal_y, 0.0); goal_ref_coords.rotate(deg2rad(goal_theta), hrp::Vector3(0,0,1)); @@ -684,7 +2288,7 @@ namespace rats // For initial double support period std::vector initial_footstep_nodes; for (size_t i = 0; i < initial_support_legs.size(); i++) { - initial_footstep_nodes.push_back(step_node(initial_support_legs.at(i), initial_support_legs_coords.at(i), 0, default_step_time, 0, 0)); + initial_footstep_nodes.push_back(step_node(initial_support_legs.at(i), initial_support_legs_coords.at(i), 0, default_step_time + tm_off, 0, 0)); } new_footstep_nodes_list.push_back(initial_footstep_nodes); } else { @@ -701,7 +2305,8 @@ namespace rats cur_vel_param.set(dp(0)/default_step_time, dp(1)/default_step_time, rad2deg(dr(2))/default_step_time); append_footstep_list_velocity_mode(new_footstep_nodes_list, cur_vel_param); start_ref_coords = new_footstep_nodes_list.back().front().worldcoords; - start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[new_footstep_nodes_list.back().front().l_r] * -1.0); + // start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[new_footstep_nodes_list.back().front().l_r] * -1.0); + start_ref_coords.pos += start_ref_coords.rot * hrp::Vector3(footstep_param.vel_foot_offset[new_footstep_nodes_list.back().front().l_r] * -1.0); start_ref_coords.difference(dp, dr, goal_ref_coords); dp = start_ref_coords.rot.transpose() * dp; dr = start_ref_coords.rot.transpose() * dr; @@ -716,7 +2321,8 @@ namespace rats // Check align coordinates final_step_coords1 = new_footstep_nodes_list[new_footstep_nodes_list.size()-2].front().worldcoords; // Final coords in footstep_node_list coordinates final_step_coords2 = start_ref_coords; // Final coords calculated from start_ref_coords + translate pos - final_step_coords2.pos += final_step_coords2.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[new_footstep_nodes_list[new_footstep_nodes_list.size()-2].front().l_r]); + // final_step_coords2.pos += final_step_coords2.rot * hrp::Vector3(footstep_param.leg_default_translate_pos[new_footstep_nodes_list[new_footstep_nodes_list.size()-2].front().l_r]); + final_step_coords2.pos += final_step_coords2.rot * hrp::Vector3(footstep_param.vel_foot_offset[new_footstep_nodes_list[new_footstep_nodes_list.size()-2].front().l_r]); final_step_coords1.difference(dp, dr, final_step_coords2); if ( !(eps_eq(dp.norm(), 0.0, 1e-3*0.1) && eps_eq(dr.norm(), 0.0, deg2rad(0.5))) ) { // If final_step_coords1 != final_step_coords2, add steps to match final_step_coords1 and final_step_coords2 append_go_pos_step_nodes(start_ref_coords, calc_counter_leg_types_from_footstep_nodes(new_footstep_nodes_list.back(), all_limbs), new_footstep_nodes_list); @@ -736,22 +2342,110 @@ namespace rats step_node sn0((_swing_leg == RLEG) ? LLEG : RLEG, _support_leg_coords, lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle()); footstep_nodes_list.push_back(boost::assign::list_of(sn0)); step_node sn1(_swing_leg, _support_leg_coords, lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle()); - hrp::Vector3 trs(2.0 * footstep_param.leg_default_translate_pos[_swing_leg] + hrp::Vector3(goal_x, goal_y, goal_z)); + // hrp::Vector3 trs(2.0 * footstep_param.leg_default_translate_pos[_swing_leg] + hrp::Vector3(goal_x, goal_y, goal_z)); + hrp::Vector3 trs(2.0 * footstep_param.vel_foot_offset[_swing_leg] + hrp::Vector3(goal_x, goal_y, goal_z)); sn1.worldcoords.pos += sn1.worldcoords.rot * trs; sn1.worldcoords.rotate(deg2rad(goal_theta), hrp::Vector3(0,0,1)); footstep_nodes_list.push_back(boost::assign::list_of(sn1)); footstep_nodes_list.push_back(boost::assign::list_of(sn0)); }; + bool gait_generator::go_wheel_param_2_wheel_nodes_list (const std::vector& goal_x_list, const std::vector& v_max_list, const std::vector& a_max_list, const coordinates& start_ref_coords, const bool with_footstep) + { + wheel_nodes_list.clear(); + std::vector tmp_wheel; + tmp_wheel.reserve(4); // TODO: consider num_step + + tmp_wheel.push_back(wheel_node(start_ref_coords, 0)); // 0番目は始点 + + tmp_wheel.push_back(wheel_node(start_ref_coords, 1)); // 1番目は直線 + + coordinates abs_goal_coord = start_ref_coords; + for (size_t i = 0; i < goal_x_list.size(); i++) { + double goal_x = goal_x_list[i], v_max = v_max_list[i], a_max = a_max_list[i]; + const double wheel_dt = 5 * dt; // 10 [ms] + interpolator wheel_traj_interpolator(1, wheel_dt, interpolator::QUINTICSPLINE); + wheel_traj_interpolator.setName("GaitGenerator wheel_traj_interpolator"); + double cur_goal; + hrp::Vector3 tmp_pos; + bool is_far = true; + + if (goal_x < 0) { + v_max *= -1; + a_max *= -1; + } + double x_tran = v_max * v_max / a_max; + if (std::abs(goal_x) < 2 * std::abs(x_tran)) { + x_tran = 0.5 * goal_x; + v_max = std::sqrt(x_tran * a_max); + if (goal_x < 0) v_max *= -1; + is_far = false; + } + const double t_tran = static_cast(((5 * x_tran) / (3 * v_max)) / wheel_dt) * wheel_dt; // 速度がv_maxをぎりぎり超えない + const double x_cons = goal_x - 2 * x_tran; + const double t_cons = static_cast((x_cons / v_max) / wheel_dt) * wheel_dt; + + // std::cerr << "aaa goal_x: " << goal_x << ", " << "v_max: " << v_max << ", " << "a_max: " << a_max << ", " << "x_tran: " << x_tran << ", " << "t_tran: " << t_tran << ", " << "x_cons: " << x_cons << ", " << "t_cons: " << t_cons << std::endl; + + // first transition period + wheel_traj_interpolator.setGoal(&x_tran, &v_max, t_tran, true); + tmp_pos = abs_goal_coord.pos; + while (!wheel_traj_interpolator.isEmpty()) { + wheel_traj_interpolator.get(&cur_goal); + abs_goal_coord.pos = tmp_pos + start_ref_coords.rot * hrp::Vector3(cur_goal, 0.0, 0.0); + tmp_wheel.push_back(wheel_node(abs_goal_coord, wheel_dt)); + } + + // linear period + if (is_far) { + tmp_pos = abs_goal_coord.pos; + abs_goal_coord.pos = tmp_pos + start_ref_coords.rot * hrp::Vector3(x_cons, 0.0, 0.0); + tmp_wheel.push_back(wheel_node(abs_goal_coord, t_cons - 1e-10)); // why need offset? + } + + // last transition period + cur_goal = 0.0; + wheel_traj_interpolator.set(&cur_goal, &v_max); + wheel_traj_interpolator.setGoal(&x_tran, t_tran, true); + // TODO: the same code as first transition period + tmp_pos = abs_goal_coord.pos; + while (!wheel_traj_interpolator.isEmpty()) { + wheel_traj_interpolator.get(&cur_goal); + abs_goal_coord.pos = tmp_pos + start_ref_coords.rot * hrp::Vector3(cur_goal, 0.0, 0.0); + tmp_wheel.push_back(wheel_node(abs_goal_coord, wheel_dt)); + } + } + + if (with_footstep) { + tmp_wheel.push_back(wheel_node(abs_goal_coord, 1e4)); // 歩行時は十分長い時間を追加 + } else { + tmp_wheel.push_back(wheel_node(abs_goal_coord, 1)); // 最後は直線 + } + + wheel_nodes_list.push_back(tmp_wheel); + + return true; + } + void gait_generator::initialize_velocity_mode (const coordinates& _ref_coords, const double vel_x, const double vel_y, const double vel_theta, const std::vector& current_legs) { - velocity_mode_flg = VEL_DOING; + if (!is_emergency_step) { + velocity_mode_flg = VEL_DOING; + } + orig_default_step_time = default_step_time; + orig_default_double_support_static_ratio_before = default_double_support_ratio_before; + orig_default_double_support_static_ratio_after = default_double_support_ratio_after; + orig_min_time = min_time; /* initialize */ clear_footstep_nodes_list(); set_velocity_param (vel_x, vel_y, vel_theta); + if (is_emergency_step) default_step_time = emergency_step_time[0]; append_go_pos_step_nodes(_ref_coords, current_legs); + if (is_emergency_step) default_step_time = emergency_step_time[1]; + append_footstep_list_velocity_mode(); + if (is_emergency_step) default_step_time = emergency_step_time[2]; append_footstep_list_velocity_mode(); append_footstep_list_velocity_mode(); append_footstep_list_velocity_mode(); @@ -762,30 +2456,56 @@ namespace rats if (velocity_mode_flg == VEL_DOING) velocity_mode_flg = VEL_ENDING; }; + void gait_generator::finalize_velocity_mode2 () + { + if (velocity_mode_flg == VEL_DOING) velocity_mode_flg = VEL_IDLING; + }; + void gait_generator::calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector& sup_fns, const velocity_mode_parameter& cur_vel_param) const { ref_coords = sup_fns.front().worldcoords; - hrp::Vector3 tmpv(footstep_param.leg_default_translate_pos[sup_fns.front().l_r] * -1.0); /* not fair to every support legs */ + // hrp::Vector3 tmpv(footstep_param.leg_default_translate_pos[sup_fns.front().l_r] * -1.0); /* not fair to every support legs */ + hrp::Vector3 tmpv(footstep_param.vel_foot_offset[sup_fns.front().l_r] * -1.0); /* not fair to every support legs */ ref_coords.pos += ref_coords.rot * tmpv; double dx = cur_vel_param.velocity_x + offset_vel_param.velocity_x, dy = cur_vel_param.velocity_y + offset_vel_param.velocity_y; dth = cur_vel_param.velocity_theta + offset_vel_param.velocity_theta; + //std::cerr << "Before limit dx " << dx << " dy " << dy << " dth " << dth << std::endl; /* velocity limitation by stride parameters <- this should be based on footstep candidates */ - dx = std::max(-1 * footstep_param.stride_bwd_x / default_step_time, std::min(footstep_param.stride_fwd_x / default_step_time, dx )); - dy = std::max(-1 * footstep_param.stride_y / default_step_time, std::min(footstep_param.stride_y / default_step_time, dy )); - dth = std::max(-1 * footstep_param.stride_theta / default_step_time, std::min(footstep_param.stride_theta / default_step_time, dth)); - /* inside step limitation */ - if (use_inside_step_limitation) { - if (cur_vel_param.velocity_y > 0) { - if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) dy *= 0.5; + if (default_stride_limitation_type == SQUARE) { + dth = std::max(-1 * footstep_param.stride_outside_theta / default_step_time, std::min(footstep_param.stride_outside_theta / default_step_time, dth)); + } else if (default_stride_limitation_type == CIRCLE) { + dth = std::max(-1 * stride_limitation_for_circle_type[2] / default_step_time, std::min(stride_limitation_for_circle_type[2] / default_step_time, dth)); + } + if (default_stride_limitation_type == SQUARE) { + dx = std::max(-1 * footstep_param.stride_bwd_x / default_step_time, std::min(footstep_param.stride_fwd_x / default_step_time, dx )); + dy = std::max(-1 * footstep_param.stride_outside_y / default_step_time, std::min(footstep_param.stride_outside_y / default_step_time, dy )); + /* inside step limitation */ + if (use_inside_step_limitation) { + if (dy > 0) { + // If dy>0 (== leftward step) and LLEG/LARM support, do inside limitation + if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) { + dy = std::min(footstep_param.stride_inside_y / default_step_time, dy); + } } else { - if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) dy *= 0.5; + // If dy<=0 (== rightward step) and RLEG/RARM support, do inside limitation + if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) { + dy = std::max(-1 * footstep_param.stride_inside_y / default_step_time, dy); + } } - if (cur_vel_param.velocity_theta > 0) { - if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) dth *= 0.5; + if (dth > 0) { + // If dth>0 (== leftward turn step) and LLEG/LARM support, do inside limitation + if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == LLEG || &boost::lambda::_1->* &step_node::l_r == LARM)) > 0) { + dth = std::min(footstep_param.stride_inside_theta / default_step_time, dth); + } } else { - if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) dth *= 0.5; + // If dth<=0 (== rightward turn step) and RLEG/RARM support, do inside limitation + if (std::count_if(sup_fns.begin(), sup_fns.end(), (&boost::lambda::_1->* &step_node::l_r == RLEG || &boost::lambda::_1->* &step_node::l_r == RARM)) > 0) { + dth = std::max(-1 * footstep_param.stride_inside_theta / default_step_time, dth); + } } + } } + //std::cerr << "After Limit dx " << dx << " dy " << dy << " dth " << dth << std::endl; trans = hrp::Vector3(dx * default_step_time, dy * default_step_time, 0); dth = deg2rad(dth * default_step_time); }; @@ -805,6 +2525,7 @@ namespace rats ref_coords.pos += ref_coords.rot * trans; ref_coords.rotate(dth, hrp::Vector3(0,0,1)); append_go_pos_step_nodes(ref_coords, calc_counter_leg_types_from_footstep_nodes(_footstep_nodes_list.back(), all_limbs), _footstep_nodes_list); + if (default_stride_limitation_type == CIRCLE) limit_stride(_footstep_nodes_list[_footstep_nodes_list.size()-1].front(), _footstep_nodes_list[_footstep_nodes_list.size()-2].front(), stride_limitation_for_circle_type); }; void gait_generator::calc_next_coords_velocity_mode (std::vector< std::vector >& ret_list, const size_t idx, const size_t future_step_num) @@ -831,15 +2552,17 @@ namespace rats } for (size_t j = 0; j < forcused_sup_legs.size(); j++) { ret.push_back(step_node(forcused_sup_legs.at(j), ref_coords, 0, 0, 0, 0)); - ret[j].worldcoords.pos += ret[j].worldcoords.rot * footstep_param.leg_default_translate_pos[forcused_sup_legs.at(j)]; + // ret[j].worldcoords.pos += ret[j].worldcoords.rot * footstep_param.leg_default_translate_pos[forcused_sup_legs.at(j)]; + ret[j].worldcoords.pos += ret[j].worldcoords.rot * footstep_param.vel_foot_offset[forcused_sup_legs.at(j)]; + if (default_stride_limitation_type == CIRCLE) limit_stride(ret[j], (i == 0 ? footstep_nodes_list[idx-1].at(j) : ret_list[i -1].at(j)), stride_limitation_for_circle_type); } ret_list.push_back(ret); } }; - void gait_generator::overwrite_refzmp_queue(const std::vector< std::vector >& fnsl) + void gait_generator::overwrite_refzmp_queue(const std::vector< std::vector >& fnsl, const hrp::Vector3& cur_cog, const hrp::Vector3& cur_cogvel, const hrp::Vector3& cur_refcog, const hrp::Vector3& cur_refcogvel, const hrp::Vector3& target_cog) { - size_t idx = get_overwritable_index(); + size_t idx = lcg.get_footstep_index(); footstep_nodes_list.erase(footstep_nodes_list.begin()+idx, footstep_nodes_list.end()); /* add new next steps ;; the number of next steps is fnsl.size() */ @@ -851,19 +2574,9 @@ namespace rats /* Update refzmp_generator */ /* Remove refzmp after idx for allocation of new refzmp by push_refzmp_from_footstep_nodes */ rg.remove_refzmp_cur_list_over_length(idx); - /* Remove refzmp in preview contoroller queue */ - if (overwritable_footstep_index_offset == 0) { - preview_controller_ptr->remove_preview_queue(); // Remove all queue - } else { - preview_controller_ptr->remove_preview_queue(lcg.get_lcg_count()); // Remove queue except current footstep. ZMP queue for current footstep remains - } /* reset index and counter */ rg.set_indices(idx); - if (overwritable_footstep_index_offset == 0) { - rg.set_refzmp_count(lcg.get_lcg_count()); // Start refzmp_count from current remaining footstep count of swinging. - } else { - rg.set_refzmp_count(static_cast(fnsl[0][0].step_time/dt)); // Start refzmp_count from step length of first overwrite step - } + rg.set_refzmp_count(lcg.get_lcg_count()); // Start refzmp_count from current remaining footstep count of swinging. /* reset refzmp */ for (size_t i = 0; i < fnsl.size(); i++) { if (emergency_flg == EMERGENCY_STOP) @@ -876,17 +2589,47 @@ namespace rats lcg.get_swing_leg_dst_steps_idx(footstep_nodes_list.size()-1), lcg.get_support_leg_steps_idx(footstep_nodes_list.size()-1)); } else { - rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list[idx+i], lcg.get_support_leg_steps_idx(idx+i)); + std::vector tmp_swing_leg_src_steps; + lcg.calc_swing_leg_src_steps(tmp_swing_leg_src_steps, footstep_nodes_list, idx+i); + toe_heel_types tht(thtc.check_toe_heel_type_from_swing_support_coords(tmp_swing_leg_src_steps.front().worldcoords, lcg.get_support_leg_steps_idx(idx+i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x()), + thtc.check_toe_heel_type_from_swing_support_coords(lcg.get_swing_leg_dst_steps_idx(idx+i).front().worldcoords, lcg.get_support_leg_steps_idx(idx+i).front().worldcoords, lcg.get_toe_pos_offset_x(), lcg.get_heel_pos_offset_x())); + if (i == 0) tht = rg.get_cur_toe_heel_types(); + rg.push_refzmp_from_footstep_nodes_for_single(footstep_nodes_list[idx+i], lcg.get_support_leg_steps_idx(idx+i), tht); } } } - /* fill preview controller queue by new refzmp */ - hrp::Vector3 rzmp; - while ( !solved ) { + /* Overwrite refzmp index in preview contoroller queue */ + size_t queue_size = preview_controller_ptr->get_preview_queue_size(); + size_t overwrite_idx = 0; // Overwrite all queue + if (is_preview) { + /* fill preview controller queue by new refzmp */ + hrp::Vector3 rzmp; + bool refzmp_exist_p; std::vector sfzos; - bool refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after); - solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, refzmp_exist_p); - rg.update_refzmp(footstep_nodes_list); + for (size_t i = overwrite_idx; i < queue_size - 1; i++) { + refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after); + preview_controller_ptr->set_preview_queue(rzmp, sfzos, i+1); + if (refzmp_exist_p) { + prev_que_rzmp = rzmp; + prev_que_sfzos = sfzos; + } + rg.update_refzmp(); + sfzos.clear(); + } + finalize_count = 0; + refzmp_exist_p = rg.get_current_refzmp(rzmp, sfzos, default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after); + if (!refzmp_exist_p) { + finalize_count++; + rzmp = prev_que_rzmp; + sfzos = prev_que_sfzos; + } else { + prev_que_rzmp = rzmp; + prev_que_sfzos = sfzos; + } + solved = preview_controller_ptr->update(refzmp, cog, swing_foot_zmp_offsets, rzmp, sfzos, (refzmp_exist_p || finalize_count < preview_controller_ptr->get_delay()-default_step_time/dt)); + rg.update_refzmp(); + } else { + update_foot_guided_controller(solved, cur_cog, cur_cogvel, cur_refcog, cur_refcogvel, target_cog); } }; @@ -908,4 +2651,3 @@ namespace rats return ret; }; } - diff --git a/rtc/AutoBalancer/GaitGenerator.h b/rtc/AutoBalancer/GaitGenerator.h index 7f56a295707..7fd689809de 100644 --- a/rtc/AutoBalancer/GaitGenerator.h +++ b/rtc/AutoBalancer/GaitGenerator.h @@ -2,6 +2,7 @@ #ifndef GAITGENERATOR_H #define GAITGENERATOR_H #include "PreviewController.h" +#include "FootGuidedController.h" #include "../ImpedanceController/RatsMatrix.h" #include "interpolator.h" #include @@ -9,6 +10,12 @@ #include #include #include +#include "../TorqueFilter/IIRFilter.h" +#include "AutoBalancerService_impl.h" + +#ifdef FOR_TESTGAITGENERATOR +#warning "Compile for testGaitGenerator" +#endif // FOR_TESTGAITGENERATOR namespace rats { @@ -16,10 +23,15 @@ namespace rats const double ratio, const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double default_top_ratio = 0.5); - void multi_mid_coords (coordinates& mid_coords, const std::vector& cs); + void multi_mid_coords (coordinates& mid_coords, const std::vector& cs, const double eps = 0.001); enum orbit_type {SHUFFLING, CYCLOID, RECTANGLE, STAIR, CYCLOIDDELAY, CYCLOIDDELAYKICK, CROSS}; enum leg_type {RLEG, LLEG, RARM, LARM, BOTH, ALL}; + enum stride_limitation_type {SQUARE, CIRCLE}; + enum swing_phase_type {LIFTOFF, TOUCHDOWN}; + enum WalkingPhase {DOUBLE_BEFORE, DOUBLE_AFTER, SINGLE}; + enum StepNumPhase {FIRST, AFTER_FIRST, BEFORE2_LAST, BEFORE_LAST, LAST, NORMAL}; + std::string leg_type_to_leg_type_string (const leg_type l_r); struct step_node { @@ -61,19 +73,30 @@ namespace rats }; }; + struct wheel_node + { + coordinates worldcoords; + double time; + wheel_node () : worldcoords(coordinates()), time() {}; + wheel_node (const coordinates& _worldcoords, const double _time) + : worldcoords(_worldcoords), time(_time) {}; + }; + /* footstep parameter */ struct footstep_parameter { /* translate pos is translate position of a leg from default foot_midcoords - * vector -> (list rleg-pos[mm] lleg-pos[mm] ) + * vector -> (list rleg-pos[m] lleg-pos[m] ) */ - std::vector leg_default_translate_pos; - /* stride params indicate max stride ( [mm], [mm], [deg] ) */ - double stride_fwd_x, stride_y, stride_theta, stride_bwd_x; + std::vector leg_default_translate_pos, vel_foot_offset; + /* stride params indicate max stride */ + double stride_fwd_x/*[m]*/, stride_outside_y/*[m]*/, stride_outside_theta/*[deg]*/, stride_bwd_x/*[m]*/, stride_inside_y/*[m]*/, stride_inside_theta/*[deg]*/; footstep_parameter (const std::vector& _leg_pos, - const double _stride_fwd_x, const double _stride_y, const double _stride_theta, const double _stride_bwd_x) - : leg_default_translate_pos(_leg_pos), - stride_fwd_x(_stride_fwd_x), stride_y(_stride_y), stride_theta(_stride_theta), stride_bwd_x(_stride_bwd_x) {}; + const double _stride_fwd_x, const double _stride_outside_y, const double _stride_outside_theta, + const double _stride_bwd_x, const double _stride_inside_y, const double _stride_inside_theta) + : leg_default_translate_pos(_leg_pos), vel_foot_offset(_leg_pos), + stride_fwd_x(_stride_fwd_x), stride_outside_y(_stride_outside_y), stride_outside_theta(_stride_outside_theta), + stride_bwd_x(_stride_bwd_x), stride_inside_y(_stride_inside_y), stride_inside_theta(_stride_inside_theta) {}; }; /* velocity parameter for velocity mode */ @@ -92,15 +115,26 @@ namespace rats }; /* Phase name of toe heel contact. - * SOLE0 : Sole contact (start). Foot angle is 0. - * SOLE2TOE : Transition of foot angle (0 -> toe_angle). - * TOE2SOLE : Transition of foot angle (toe_angle -> 0). + * SOLE0 : Sole contact (start). Foot angle is 0. ZMP transition is ee -> toe. + * SOLE2TOE : Transition of foot angle (0 -> toe_angle). ZMP is on toe. + * TOE2SOLE : Transition of foot angle (toe_angle -> 0). ZMP is on toe. * SOLE1 : Foot_angle is 0. - * SOLE2HEEL : Transition of foot angle (0 -> -1 * heel_angle). - * HEEL2SOLE : Transition of foot angle (-1 * heel_angle -> 0). - * SOLE2 : Sole contact (end). Foot angle is 0. + * SOLE2HEEL : Transition of foot angle (0 -> -1 * heel_angle). ZMP is on heel. + * HEEL2SOLE : Transition of foot angle (-1 * heel_angle -> 0). ZMP is on heel. + * SOLE2 : Sole contact (end). Foot angle is 0. ZMP transition is heel -> ee. */ enum toe_heel_phase {SOLE0, SOLE2TOE, TOE2SOLE, SOLE1, SOLE2HEEL, HEEL2SOLE, SOLE2, NUM_TH_PHASES}; + static double no_using_toe_heel_ratio = 1.0; // Ratio not to use toe and heel contact + static double using_toe_heel_ratio = 0.0; // Ratio to use toe and heel contact + + enum toe_heel_type {SOLE, TOE, HEEL}; + struct toe_heel_types + { + toe_heel_type src_type, dst_type; + toe_heel_types (const toe_heel_type _src_type = SOLE, const toe_heel_type _dst_type = SOLE) : src_type(_src_type), dst_type(_dst_type) + { + }; + }; /* Manager for toe heel phase. */ class toe_heel_phase_counter @@ -114,6 +148,7 @@ namespace rats ratio_sum += toe_heel_phase_ratio[i]; toe_heel_phase_count[i] = static_cast(one_step_count * ratio_sum); } + return true; }; public: toe_heel_phase_counter () : one_step_count(0) @@ -169,7 +204,6 @@ namespace rats { for (size_t i = 0; i < NUM_TH_PHASES; i++) ratio[i] = toe_heel_phase_ratio[i]; }; - int get_NUM_TH_PHASES () const { return NUM_TH_PHASES; }; // functions for checking phase and calculating phase time and ratio bool is_phase_starting (const size_t current_count, const toe_heel_phase _phase) const { @@ -184,20 +218,81 @@ namespace rats return (current_count < toe_heel_phase_count[phase1]); }; bool is_no_SOLE1_phase () const { return toe_heel_phase_count[TOE2SOLE] == toe_heel_phase_count[SOLE1]; }; + // Calculate period [s] between start_phase and goal_phase double calc_phase_period (const toe_heel_phase start_phase, const toe_heel_phase goal_phase, const double _dt) const { return _dt * (toe_heel_phase_count[goal_phase]-toe_heel_phase_count[start_phase]); }; + // Calculate ratio between start_phase and goal_phase. + // If current_count is 0->goal_phase_count, start_phase : 0 -> goal_phase : 1 double calc_phase_ratio (const size_t current_count, const toe_heel_phase start_phase, const toe_heel_phase goal_phase) const { return static_cast(current_count-toe_heel_phase_count[start_phase]) / (toe_heel_phase_count[goal_phase]-toe_heel_phase_count[start_phase]); }; + // Calculate ratio to goal_phase. + // If current_count is 0->goal_phase_count, start : 0 -> goal_phase : 1 double calc_phase_ratio (const size_t current_count, const toe_heel_phase goal_phase) const { return static_cast(current_count) / (toe_heel_phase_count[goal_phase]); }; + // Calculate ratio for toe heel transition. + // If toe or heel are used, ratio is 0.0. Otherwise, ratio is (0.0, 1.0]. + // We assume current_count is 0->goal_phase_count. + double calc_phase_ratio_for_toe_heel_transition (const size_t current_count) const + { + if (is_between_phases(current_count, SOLE0)) { + // ratio : 1 -> 0 + return 1-calc_phase_ratio(current_count, SOLE0); + } else if (is_between_phases(current_count, HEEL2SOLE, SOLE2) || toe_heel_phase_count[SOLE2] == current_count) { + // ratio : 0 -> 1 + return calc_phase_ratio(current_count, HEEL2SOLE, SOLE2); + } else { + // If using toe or heel, 0 + return using_toe_heel_ratio; + } + }; }; + /* Checker for toe heel type. */ + class toe_heel_type_checker + { + private: + double toe_check_thre, heel_check_thre; // [m] + public: + toe_heel_type_checker () : toe_check_thre(0), heel_check_thre(0) + { + }; + toe_heel_type_checker (const double _toe_check_thre, const double _heel_check_thre) : toe_check_thre(_toe_check_thre), heel_check_thre(_heel_check_thre) + { + }; + toe_heel_type check_toe_heel_type_from_swing_support_coords (const coordinates& swing_coords, const coordinates& support_coords, const double toe_pos_offset_x, const double heel_pos_offset_x) const + { + hrp::Vector3 local_toe_pos = support_coords.rot.transpose() * (swing_coords.rot * hrp::Vector3(toe_pos_offset_x,0,0) + swing_coords.pos - support_coords.pos); + hrp::Vector3 local_heel_pos = support_coords.rot.transpose() * (swing_coords.rot * hrp::Vector3(heel_pos_offset_x,0,0) + swing_coords.pos - support_coords.pos); + if (local_toe_pos(2) < -50*1e-3 && (local_toe_pos(0) + toe_check_thre < 0 || local_heel_pos(0) - heel_check_thre > 0) ) { + return TOE; + } else if (local_toe_pos(0) + toe_check_thre < 0) { + return TOE; + } else if (local_heel_pos(0) - heel_check_thre > 0) { + return HEEL; + } else { + return SOLE; + } + }; + void print_param (const std::string print_str = "") const + { + std::cerr << "[" << print_str << "] toe_check_thre = " << toe_check_thre << ", heel_check_thre = " << heel_check_thre << std::endl; + }; + // Setter + void set_toe_check_thre (const double _toe_check_thre) { toe_check_thre = _toe_check_thre; }; + void set_heel_check_thre (const double _heel_check_thre) { heel_check_thre = _heel_check_thre; }; + // Getter + double get_toe_check_thre () const { return toe_check_thre; }; + double get_heel_check_thre () const { return heel_check_thre; }; + }; + + double set_value_according_to_toe_heel_type (const toe_heel_type tht, const double toe_value, const double heel_value, const double default_value); + /* refzmp_generator to generate current refzmp from footstep_node_list */ class refzmp_generator { @@ -209,12 +304,14 @@ namespace rats std::vector< std::vector > foot_x_axises_list; // Swing foot x axis list according to refzmp_cur_list std::vector< std::vector > swing_leg_types_list; // Swing leg list according to refzmp_cur_list std::vector step_count_list; // Swing leg list according to refzmp_cur_list + std::vector toe_heel_types_list; + toe_heel_types cur_toe_heel_types; std::vector default_zmp_offsets; /* list of RLEG and LLEG */ size_t refzmp_index, refzmp_count, one_step_count; double toe_zmp_offset_x, heel_zmp_offset_x; // [m] double dt; - toe_heel_phase_counter* thp_ptr; - bool use_toe_heel_transition; + toe_heel_phase_counter thp; + bool use_toe_heel_transition, use_toe_heel_auto_set; boost::shared_ptr zmp_weight_interpolator; void calc_current_refzmp (hrp::Vector3& ret, std::vector& swing_foot_zmp_offsets, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const double default_double_support_static_ratio_before, const double default_double_support_static_ratio_after); const bool is_start_double_support_phase () const { return refzmp_index == 0; }; @@ -224,25 +321,24 @@ namespace rats #ifndef HAVE_MAIN public: #endif - refzmp_generator(toe_heel_phase_counter* _thp_ptr, const double _dt) - : refzmp_cur_list(), foot_x_axises_list(), swing_leg_types_list(), step_count_list(), default_zmp_offsets(), + refzmp_generator(const double _dt) + : refzmp_cur_list(), foot_x_axises_list(), swing_leg_types_list(), step_count_list(), toe_heel_types_list(), default_zmp_offsets(), refzmp_index(0), refzmp_count(0), one_step_count(0), toe_zmp_offset_x(0), heel_zmp_offset_x(0), dt(_dt), - thp_ptr(_thp_ptr), use_toe_heel_transition(false) + thp(), use_toe_heel_transition(false), use_toe_heel_auto_set(false) { default_zmp_offsets.push_back(hrp::Vector3::Zero()); default_zmp_offsets.push_back(hrp::Vector3::Zero()); default_zmp_offsets.push_back(hrp::Vector3::Zero()); default_zmp_offsets.push_back(hrp::Vector3::Zero()); double zmp_weight_initial_value[4] = {1.0, 1.0, 0.1, 0.1}; - zmp_weight_map = boost::assign::map_list_of(RLEG, zmp_weight_initial_value[0])(LLEG, zmp_weight_initial_value[1])(RARM, zmp_weight_initial_value[2])(LARM, zmp_weight_initial_value[3]); + zmp_weight_map = boost::assign::map_list_of(RLEG, zmp_weight_initial_value[0])(LLEG, zmp_weight_initial_value[1])(RARM, zmp_weight_initial_value[2])(LARM, zmp_weight_initial_value[3]).convert_to_container< std::map > (); zmp_weight_interpolator = boost::shared_ptr(new interpolator(4, dt)); zmp_weight_interpolator->set(zmp_weight_initial_value); /* set initial value */ zmp_weight_interpolator->setName("GaitGenerator zmp_weight_interpolator"); }; ~refzmp_generator() { - thp_ptr = NULL; }; void remove_refzmp_cur_list_over_length (const size_t len) { @@ -250,6 +346,10 @@ namespace rats while ( foot_x_axises_list.size() > len) foot_x_axises_list.pop_back(); while ( swing_leg_types_list.size() > len) swing_leg_types_list.pop_back(); while ( step_count_list.size() > len) step_count_list.pop_back(); + while ( toe_heel_types_list.size() > len) { + if (toe_heel_types_list.size() == len + 1) cur_toe_heel_types = toe_heel_types_list.back(); + toe_heel_types_list.pop_back(); + } }; void reset (const size_t _refzmp_count) { @@ -260,30 +360,35 @@ namespace rats foot_x_axises_list.clear(); swing_leg_types_list.clear(); step_count_list.clear(); + toe_heel_types_list.clear(); + thp.set_one_step_count(one_step_count); }; void push_refzmp_from_footstep_nodes_for_dual (const std::vector& fns, const std::vector& _support_leg_steps, const std::vector& _swing_leg_steps); - void push_refzmp_from_footstep_nodes_for_single (const std::vector& fns, const std::vector& _support_leg_stepsconst); - void update_refzmp (const std::vector< std::vector >& fnsl); + void push_refzmp_from_footstep_nodes_for_single (const std::vector& fns, const std::vector& _support_leg_steps, const toe_heel_types& tht); + void update_refzmp (); // setter void set_indices (const size_t idx) { refzmp_index = idx; }; + void set_one_step_count (const size_t d_cnt) { one_step_count += d_cnt; }; void set_refzmp_count(const size_t _refzmp_count) { refzmp_count = _refzmp_count; }; void set_default_zmp_offsets(const std::vector& tmp) { default_zmp_offsets = tmp; }; void set_toe_zmp_offset_x (const double _off) { toe_zmp_offset_x = _off; }; void set_heel_zmp_offset_x (const double _off) { heel_zmp_offset_x = _off; }; - void set_use_toe_heel_transition (const double _u) { use_toe_heel_transition = _u; }; + void set_use_toe_heel_transition (const bool _u) { use_toe_heel_transition = _u; }; + void set_use_toe_heel_auto_set (const bool _u) { use_toe_heel_auto_set = _u; }; void set_zmp_weight_map (const std::map _map) { double zmp_weight_array[4] = {_map.find(RLEG)->second, _map.find(LLEG)->second, _map.find(RARM)->second, _map.find(LARM)->second}; if (zmp_weight_interpolator->isEmpty()) { zmp_weight_interpolator->clear(); double zmp_weight_initial_value[4] = {zmp_weight_map[RLEG], zmp_weight_map[LLEG], zmp_weight_map[RARM], zmp_weight_map[LARM]}; zmp_weight_interpolator->set(zmp_weight_initial_value); - zmp_weight_interpolator->go(zmp_weight_array, 2.0, true); + zmp_weight_interpolator->setGoal(zmp_weight_array, 2.0, true); } else { std::cerr << "zmp_weight_map cannot be set because interpolating." << std::endl; } }; + bool set_toe_heel_phase_ratio (const std::vector& ratio) { return thp.set_toe_heel_phase_ratio(ratio); }; // getter bool get_current_refzmp (hrp::Vector3& rzmp, std::vector& swing_foot_zmp_offsets, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const double default_double_support_static_ratio_before, const double default_double_support_static_ratio_after) { @@ -295,14 +400,19 @@ namespace rats double get_toe_zmp_offset_x () const { return toe_zmp_offset_x; }; double get_heel_zmp_offset_x () const { return heel_zmp_offset_x; }; bool get_use_toe_heel_transition () const { return use_toe_heel_transition; }; + bool get_use_toe_heel_auto_set () const { return use_toe_heel_auto_set; }; + toe_heel_types get_cur_toe_heel_types () const { return cur_toe_heel_types; }; const std::map get_zmp_weight_map () const { return zmp_weight_map; }; void proc_zmp_weight_map_interpolation () { if (!zmp_weight_interpolator->isEmpty()) { double zmp_weight_output[4]; zmp_weight_interpolator->get(zmp_weight_output, true); - zmp_weight_map = boost::assign::map_list_of(RLEG, zmp_weight_output[0])(LLEG, zmp_weight_output[1])(RARM, zmp_weight_output[2])(LARM, zmp_weight_output[3]); + zmp_weight_map = boost::assign::map_list_of(RLEG, zmp_weight_output[0])(LLEG, zmp_weight_output[1])(RARM, zmp_weight_output[2])(LARM, zmp_weight_output[3]).convert_to_container < std::map > (); } }; +#ifdef FOR_TESTGAITGENERATOR + std::vector get_default_zmp_offsets() const { return default_zmp_offsets; }; +#endif // FOR_TESTGAITGENERATOR }; class delay_hoffarbib_trajectory_generator @@ -310,78 +420,130 @@ namespace rats private: hrp::Vector3 pos, vel, acc; // [m], [m/s], [m/s^2] double dt; // [s] + // Parameters for antecedent path generation + std::vector point_vec; + std::vector distance_vec; + std::vector sum_distance_vec; + double total_path_length; // Implement hoffarbib to configure remain_time; - void hoffarbib_interpolation (const double tmp_remain_time, const hrp::Vector3& tmp_goal) + void hoffarbib_interpolation (double& _pos, double& _vel, double& _acc, const double tmp_remain_time, const double tmp_goal, const double tmp_goal_vel = 0, const double tmp_goal_acc = 0) { - hrp::Vector3 jerk = (-9.0/ tmp_remain_time) * acc + - (-36.0 / (tmp_remain_time * tmp_remain_time)) * vel + - (60.0 / (tmp_remain_time * tmp_remain_time * tmp_remain_time)) * (tmp_goal - pos); - acc = acc + dt * jerk; - vel = vel + dt * acc; - pos = pos + dt * vel; + double jerk = (-9.0/ tmp_remain_time) * (_acc - tmp_goal_acc / 3.0) + + (-36.0 / (tmp_remain_time * tmp_remain_time)) * (tmp_goal_vel * 2.0 / 3.0 + _vel) + + (60.0 / (tmp_remain_time * tmp_remain_time * tmp_remain_time)) * (tmp_goal - _pos); + _acc = _acc + dt * jerk; + _vel = _vel + dt * _acc; + _pos = _pos + dt * _vel; }; protected: double time_offset; // [s] double final_distance_weight; + double time_offset_xy2z; // [s] size_t one_step_count, current_count, double_support_count_before, double_support_count_after; // time/dt - virtual hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) = 0; + swing_phase_type spt; + virtual double calc_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current) = 0; public: - delay_hoffarbib_trajectory_generator () : time_offset(0.35), final_distance_weight(1.0), one_step_count(0), current_count(0), double_support_count_before(0), double_support_count_after(0) {}; + delay_hoffarbib_trajectory_generator () : time_offset(0.35), final_distance_weight(1.0), time_offset_xy2z(0), one_step_count(0), current_count(0), double_support_count_before(0), double_support_count_after(0), goal_off(hrp::Vector3::Zero()), is_early_touch(false), time_smooth_offset(2.0) {}; ~delay_hoffarbib_trajectory_generator() { }; + bool is_touch_ground, is_single_walking, is_early_touch; + hrp::Vector3 goal_off; + double time_smooth_offset; void set_dt (const double _dt) { dt = _dt; }; void set_swing_trajectory_delay_time_offset (const double _time_offset) { time_offset = _time_offset; }; void set_swing_trajectory_final_distance_weight (const double _final_distance_weight) { final_distance_weight = _final_distance_weight; }; + void set_swing_trajectory_time_offset_xy2z (const double _tmp) { time_offset_xy2z = _tmp; }; void reset (const size_t _one_step_len, const double default_double_support_ratio_before, const double default_double_support_ratio_after) { one_step_count = _one_step_len; current_count = 0; double_support_count_before = (default_double_support_ratio_before*one_step_count); double_support_count_after = (default_double_support_ratio_after*one_step_count); + spt = LIFTOFF; + }; + void reset_one_step_count (const size_t d_one_step_len, const double default_double_support_ratio_before, const double default_double_support_ratio_after) + { + one_step_count += d_one_step_len; + // double_support_count_before = (default_double_support_ratio_before*one_step_count); + double_support_count_after = (default_double_support_ratio_after*one_step_count); }; void reset_all (const double _dt, const size_t _one_step_len, const double default_double_support_ratio_before, const double default_double_support_ratio_after, - const double _time_offset, const double _final_distance_weight) + const double _time_offset, const double _final_distance_weight, const double _time_offset_xy2z) { set_dt(_dt); reset (_one_step_len, default_double_support_ratio_before, default_double_support_ratio_after); set_swing_trajectory_delay_time_offset(_time_offset); set_swing_trajectory_final_distance_weight(_final_distance_weight); + set_swing_trajectory_time_offset_xy2z(_time_offset_xy2z); + vel = acc = hrp::Vector3::Zero(); }; - void get_trajectory_point (hrp::Vector3& ret, const hrp::Vector3& start, const hrp::Vector3& goal, const double height) + void get_trajectory_point (hrp::Vector3& ret, const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current = hrp::Vector3::Zero()) // TODO : support only rectangle { - if ( double_support_count_before <= current_count && current_count < one_step_count - double_support_count_after ) { // swing phase + if ( current_count <= double_support_count_before ) { // first double support phase + pos = start; + vel = hrp::Vector3::Zero(); + acc = hrp::Vector3::Zero(); + is_early_touch = false; + } else if ( current_count < one_step_count - double_support_count_after ) { // swing phase size_t swing_remain_count = one_step_count - current_count - double_support_count_after; size_t swing_one_step_count = one_step_count - double_support_count_before - double_support_count_after; - if (swing_remain_count*dt > time_offset) { // antecedent path is still interpolating - hoffarbib_interpolation (time_offset, interpolate_antecedent_path(start, goal, height, ((swing_one_step_count - swing_remain_count) / (swing_one_step_count - time_offset/dt)))); - } else if (swing_remain_count > 0) { // antecedent path already reached to goal - hoffarbib_interpolation (swing_remain_count*dt, goal); + double final_path_distance_ratio = calc_antecedent_path(start, goal, height, current); + size_t tmp_time_offset_count = time_offset/dt; + // XYZ interpolation + if (static_cast(swing_remain_count) / static_cast(swing_one_step_count) <= 0.5 && spt == LIFTOFF) spt = TOUCHDOWN; + if (swing_remain_count > tmp_time_offset_count) { // antecedent path is still interpolating + double tmp_ratio = static_cast(tmp_time_offset_count) / static_cast(swing_remain_count); // ~0 -> 1 + hrp::Vector3 tmpgoal = interpolate_antecedent_path(tmp_ratio); + for (size_t i = 0; i < 2; i++) hoffarbib_interpolation (pos(i), vel(i), acc(i), (tmp_ratio + (1 - tmp_ratio) * time_smooth_offset) * time_offset, tmpgoal(i)); // time_smooth_offset is to make xy swing movement slower at first + hoffarbib_interpolation (pos(2), vel(2), acc(2), time_offset, tmpgoal(2)); + } else { // antecedent path already reached to goal + hrp::Vector3 tmp_goal = goal; + hrp::Vector3 tmp_vel = hrp::Vector3::Zero(); + size_t tmp_count = swing_remain_count; + if (is_single_walking && !is_early_touch) { + tmp_goal += goal_off * 0.5; // 0.5 makes touch-down gentle + // tmp_vel = goal_off / ((one_step_count - double_support_count_after)*dt); + tmp_count += double_support_count_after; + } + for (size_t i = 0; i < 3; i++) hoffarbib_interpolation (pos(i), vel(i), acc(i), tmp_count*dt, tmp_goal(i), tmp_vel(i)); + if (is_early_touch) { + pos = goal; + vel = hrp::Vector3::Zero(); + acc = hrp::Vector3::Zero(); + } + } + } else { // last double support phase + size_t remain_count = one_step_count - current_count; + if (remain_count > 0 && !is_early_touch) { + hrp::Vector3 tmp_goal = goal; + if (is_single_walking) tmp_goal += goal_off; + for (size_t i = 0; i < 3; i++) hoffarbib_interpolation (pos(i), vel(i), acc(i), remain_count*dt, tmp_goal(i)); } else { pos = goal; + vel = hrp::Vector3::Zero(); + acc = hrp::Vector3::Zero(); } - } else if ( current_count < double_support_count_before ) { // first double support phase + } + if (!is_touch_ground && start == current) { pos = start; vel = hrp::Vector3::Zero(); acc = hrp::Vector3::Zero(); - } else { // last double support phase - pos = goal; - vel = hrp::Vector3::Zero(); - acc = hrp::Vector3::Zero(); } ret = pos; current_count++; }; double get_swing_trajectory_delay_time_offset () const { return time_offset; }; double get_swing_trajectory_final_distance_weight () const { return final_distance_weight; }; + double get_swing_trajectory_time_offset_xy2z () const { return time_offset_xy2z; }; // interpolate path vector // tmp_ratio : ratio value [0, 1] // org_point_vec : vector of via points // e.g., move tmp_ratio from 0 to 1 => move point from org_point_vec.front() to org_point_vec.back() - hrp::Vector3 interpolate_antecedent_path_base (const double tmp_ratio, const std::vector org_point_vec) + double calc_antecedent_path_base (const std::vector org_point_vec) { - std::vector point_vec; - std::vector distance_vec; - double total_path_length = 0; + total_path_length = 0; + point_vec.clear(); + distance_vec.clear(); point_vec.push_back(org_point_vec.front()); // remove distance-zero points for (size_t i = 0; i < org_point_vec.size()-1; i++) { @@ -394,18 +556,25 @@ namespace rats } } if ( total_path_length < 1e-5 ) { // if total path is zero, return goal point. - return org_point_vec.back(); + return 0; } // point_vec : [p0, p1, ..., pN-1, pN] // distance_vec : [ d0, ..., dN-1 ] // sum_distance_vec : [l0, l1, ..., lN-1, lN] <= lj = \Sum_{i=0}^{j-1} di - std::vector sum_distance_vec; + sum_distance_vec.clear(); sum_distance_vec.push_back(0); double tmp_dist = 0; for (size_t i = 0; i < distance_vec.size(); i++) { sum_distance_vec.push_back(tmp_dist + distance_vec[i]); tmp_dist += distance_vec[i]; } + return distance_vec.back()/total_path_length; + }; + hrp::Vector3 interpolate_antecedent_path (const double tmp_ratio) + { + if ( total_path_length < 1e-5 ) { // if total path is zero, return goal point. + return point_vec.back(); + } // select current segment in which 'tmp_ratio' is included double current_length = tmp_ratio * total_path_length; for (size_t i = 0; i < sum_distance_vec.size(); i++) { @@ -415,29 +584,54 @@ namespace rats } } // if illegal tmp-ratio - if (current_length < 0) return org_point_vec.front(); - else org_point_vec.back(); + if (current_length < 0) return point_vec.front(); + else return point_vec.back(); }; }; class rectangle_delay_hoffarbib_trajectory_generator : public delay_hoffarbib_trajectory_generator { - hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) + private: + leg_type swing_leg; + public: + rectangle_delay_hoffarbib_trajectory_generator () : delay_hoffarbib_trajectory_generator(), way_point_offset(0.05, 0.0, 0.0) {}; + ~rectangle_delay_hoffarbib_trajectory_generator () {}; + void set_swing_leg (leg_type _lr) { swing_leg = _lr; }; + void set_rectangle_trajectory_way_point_offset (const hrp::Vector3 _offset) { way_point_offset = _offset; }; + hrp::Vector3 get_rectangle_trajectory_way_point_offset() const { return way_point_offset; }; + hrp::Vector3 way_point_offset; + double calc_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current) { std::vector rectangle_path; double max_height = std::max(start(2), goal(2))+height; - rectangle_path.push_back(start); - rectangle_path.push_back(hrp::Vector3(start(0), start(1), max_height)); - rectangle_path.push_back(hrp::Vector3(goal(0), goal(1), max_height)); + hrp::Vector3 diff_vec = goal - start; + rectangle_path.push_back(current); + switch (spt) { + case LIFTOFF: + if (diff_vec(2) > 2e-2) { // 2cm + rectangle_path.push_back(hrp::Vector3(current(0), current(1), max_height) - way_point_offset(0) * diff_vec.normalized()); + } else { + rectangle_path.push_back(hrp::Vector3(current(0), current(1), max_height)); + } + if (diff_vec(0) > 5e-3) rectangle_path.back() += hrp::Vector3(0.0, (swing_leg == LLEG ? 1 : -1) * way_point_offset(1), way_point_offset(2)); + rectangle_path.push_back(hrp::Vector3(goal(0), goal(1), max_height)); + if (diff_vec(0) > 5e-3) rectangle_path.back() += hrp::Vector3(0.0, (swing_leg == LLEG ? 1 : -1) * way_point_offset(1), way_point_offset(2)); + break; + case TOUCHDOWN: + rectangle_path.push_back(hrp::Vector3(goal(0), goal(1), current(2))); + break; + default: + break; + } rectangle_path.push_back(goal); - return interpolate_antecedent_path_base(tmp_ratio, rectangle_path); + return calc_antecedent_path_base(rectangle_path); }; }; class stair_delay_hoffarbib_trajectory_generator : public delay_hoffarbib_trajectory_generator { hrp::Vector3 way_point_offset; - hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) + double calc_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current) { std::vector path; double max_height = std::max(start(2), goal(2))+height; @@ -459,7 +653,7 @@ namespace rats // path.push_back(hrp::Vector3(goal(0), goal(1), 20*1e-3+goal(2))); // } path.push_back(goal); - return interpolate_antecedent_path_base(tmp_ratio, path); + return calc_antecedent_path_base(path); }; public: stair_delay_hoffarbib_trajectory_generator () : delay_hoffarbib_trajectory_generator(), way_point_offset(hrp::Vector3(0.03, 0.0, 0.0)) {}; @@ -470,7 +664,7 @@ namespace rats class cycloid_delay_hoffarbib_trajectory_generator : public delay_hoffarbib_trajectory_generator { - hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) + double calc_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current) { std::vector cycloid_path; hrp::Vector3 tmpv, via_goal(goal); @@ -488,7 +682,7 @@ namespace rats cycloid_path.push_back(tmpv); cycloid_path.push_back(via_goal); cycloid_path.push_back(goal); - return interpolate_antecedent_path_base(tmp_ratio, cycloid_path); + return calc_antecedent_path_base(cycloid_path); }; }; @@ -502,7 +696,7 @@ namespace rats void set_cycloid_delay_kick_point_offset (const hrp::Vector3 _offset) { kick_point_offset = _offset; }; void set_start_rot (const hrp::Matrix33 _offset) { start_rot = _offset; }; hrp::Vector3 get_cycloid_delay_kick_point_offset () const { return kick_point_offset; }; - hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) + double calc_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current) { std::vector cycloid_path; hrp::Vector3 tmpv, via_goal(goal); @@ -524,7 +718,7 @@ namespace rats } cycloid_path.push_back(via_goal); cycloid_path.push_back(goal); - return interpolate_antecedent_path_base(tmp_ratio, cycloid_path); + return calc_antecedent_path_base(cycloid_path); }; }; @@ -537,7 +731,7 @@ namespace rats ~cross_delay_hoffarbib_trajectory_generator () {}; void set_swing_leg (leg_type _lr) { swing_leg = _lr; }; hrp::Vector3 way_point_offset; - hrp::Vector3 interpolate_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const double tmp_ratio) + double calc_antecedent_path (const hrp::Vector3& start, const hrp::Vector3& goal, const double height, const hrp::Vector3& current) { std::vector path; double max_height = std::max(start(2), goal(2))+height; @@ -555,7 +749,7 @@ namespace rats path.push_back(hrp::Vector3(goal(0), goal(1), 30*1e-3+goal(2))); } path.push_back(goal); - return interpolate_antecedent_path_base(tmp_ratio, path); + return calc_antecedent_path_base(path); }; }; @@ -569,38 +763,47 @@ namespace rats // Support leg coordinates. std::vector support_leg_steps; // Swing leg coordinates is interpolated from swing_leg_src_coords to swing_leg_dst_coords during swing phase. - std::vector swing_leg_steps, swing_leg_src_steps, swing_leg_dst_steps; - double default_step_height, default_top_ratio, current_step_height, swing_ratio, swing_rot_ratio, foot_midcoords_ratio, dt, current_toe_angle, current_heel_angle; - double time_offset, final_distance_weight; + std::vector swing_leg_steps, swing_leg_src_steps, swing_leg_dst_steps, current_swing_leg_steps; + double default_step_height, default_top_ratio, current_step_height, swing_ratio, dt, current_toe_angle, current_heel_angle; + double time_offset, final_distance_weight, time_offset_xy2z; std::vector current_swing_time; // Index for current footstep. footstep_index should be [0,footstep_node_list.size()]. Current footstep is footstep_node_list[footstep_index]. size_t footstep_index; // one_step_count is total counter num of current steps (= step_time/dt). lcg_count is counter for lcg. During one step, lcg_count decreases from one_step_count to 0. - size_t lcg_count, one_step_count, next_one_step_count; + size_t lcg_count, one_step_count, next_one_step_count, swing_rot_count_ratio; // Current support leg std::vector support_leg_types, swing_leg_types; orbit_type default_orbit_type; bool is_swing_phase; // Foot trajectory generators std::vector rdtg; + hrp::Vector3 rectangle_way_point_offset, rectangle_goal_off; stair_delay_hoffarbib_trajectory_generator sdtg; std::vector cdtg; cycloid_delay_kick_hoffarbib_trajectory_generator cdktg; cross_delay_hoffarbib_trajectory_generator crdtg; - toe_heel_phase_counter* thp_ptr; - interpolator* foot_ratio_interpolator; - interpolator* swing_foot_rot_ratio_interpolator; + toe_heel_phase_counter thp; + interpolator* foot_midcoords_interpolator; + coordinates swing_support_midcoords; + // Map for interpolator of each swing foot rot interpolation. In constructor, prepared for all limbs. In control loop, swing foot element is used. + std::map swing_foot_rot_interpolator; // Parameters for toe-heel contact interpolator* toe_heel_interpolator; - double toe_pos_offset_x, heel_pos_offset_x, toe_angle, heel_angle, foot_dif_rot_angle; - bool use_toe_joint; - void calc_current_swing_leg_steps (std::vector& rets, const double step_height, const double _current_toe_angle, const double _current_heel_angle); + double toe_pos_offset_x, heel_pos_offset_x, toe_angle, heel_angle, foot_dif_rot_angle, toe_heel_dif_angle; + bool use_toe_joint, use_toe_heel_auto_set; + toe_heel_type current_src_toe_heel_type, current_dst_toe_heel_type; + std::vector act_contact_states; + bool is_touch_ground, use_act_states, is_single_walking, is_stop_early_foot, use_force_sensor; + double rectangle_time_smooth_offset; + std::map touch_ground_count; + void calc_current_swing_foot_rot (std::map& tmp_swing_foot_rot, const double _default_double_support_ratio_before, const double _default_double_support_ratio_after); + void calc_current_swing_leg_steps (std::vector& rets, const double step_height, const double _current_toe_angle, const double _current_heel_angle, const double _default_double_support_ratio_before, const double _default_double_support_ratio_after); double calc_interpolated_toe_heel_angle (const toe_heel_phase start_phase, const toe_heel_phase goal_phase, const double start, const double goal); void modif_foot_coords_for_toe_heel_phase (coordinates& org_coords, const double _current_toe_angle, const double _current_heel_angle); void cycloid_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height) const; void rectangle_midcoords (coordinates& ret, const coordinates& start, - const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx); + const coordinates& goal, const double height, const size_t swing_trajectory_generator_idx, const coordinates& current_coords, leg_type lr); void stair_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height); void cycloid_delay_midcoords (coordinates& ret, const coordinates& start, @@ -610,53 +813,65 @@ namespace rats void cross_delay_midcoords (coordinates& ret, const coordinates& start, const coordinates& goal, const double height, leg_type lr); void calc_ratio_from_double_support_ratio (const double default_double_support_ratio_before, const double default_double_support_ratio_after); + void calc_swing_support_mid_coords (); #ifndef HAVE_MAIN public: #endif - leg_coords_generator(const double _dt, toe_heel_phase_counter* _thp_ptr) + leg_coords_generator(const double _dt) : support_leg_steps(), swing_leg_steps(), swing_leg_src_steps(), swing_leg_dst_steps(), - default_step_height(0.05), default_top_ratio(0.5), current_step_height(0.0), swing_ratio(0), swing_rot_ratio(0), foot_midcoords_ratio(0), dt(_dt), + default_step_height(0.05), default_top_ratio(0.5), current_step_height(0.0), swing_ratio(0), dt(_dt), current_toe_angle(0), current_heel_angle(0), - time_offset(0.35), final_distance_weight(1.0), - footstep_index(0), lcg_count(0), default_orbit_type(CYCLOID), - rdtg(), cdtg(), - thp_ptr(_thp_ptr), - foot_ratio_interpolator(NULL), swing_foot_rot_ratio_interpolator(NULL), toe_heel_interpolator(NULL), - toe_pos_offset_x(0.0), heel_pos_offset_x(0.0), toe_angle(0.0), heel_angle(0.0), foot_dif_rot_angle(0.0), use_toe_joint(false) + time_offset(0.35), final_distance_weight(1.0), time_offset_xy2z(0), + footstep_index(0), lcg_count(0), swing_rot_count_ratio(0.1), default_orbit_type(CYCLOID), + rdtg(), rectangle_way_point_offset(0.05, 0.0, 0.0), rectangle_goal_off(hrp::Vector3::Zero()), cdtg(), + thp(), use_act_states(true), is_stop_early_foot(false), use_force_sensor(true), rectangle_time_smooth_offset(2.0), + foot_midcoords_interpolator(NULL), swing_foot_rot_interpolator(), toe_heel_interpolator(NULL), + toe_pos_offset_x(0.0), heel_pos_offset_x(0.0), toe_angle(0.0), heel_angle(0.0), foot_dif_rot_angle(0.0), toe_heel_dif_angle(0.0), use_toe_joint(false), use_toe_heel_auto_set(false), + current_src_toe_heel_type(SOLE), current_dst_toe_heel_type(SOLE) { - support_leg_types = boost::assign::list_of(RLEG); - swing_leg_types = boost::assign::list_of(LLEG); - current_swing_time = boost::assign::list_of(0.0)(0.0)(0.0)(0.0); + support_leg_types.assign (1, RLEG); + swing_leg_types.assign (1, LLEG); + current_swing_time.assign (4, 0.0); sdtg.set_dt(dt); cdktg.set_dt(dt); crdtg.set_dt(dt); - if (foot_ratio_interpolator == NULL) foot_ratio_interpolator = new interpolator(1, dt); - if (swing_foot_rot_ratio_interpolator == NULL) swing_foot_rot_ratio_interpolator = new interpolator(1, dt); + if (foot_midcoords_interpolator == NULL) foot_midcoords_interpolator = new interpolator(6, dt); // POS + RPY + std::vector tmp_leg_types = boost::assign::list_of(RLEG)(LLEG)(RARM)(LARM); + for (size_t i = 0; i < tmp_leg_types.size(); i++) { + if ( swing_foot_rot_interpolator.find(tmp_leg_types[i]) == swing_foot_rot_interpolator.end() ) { + swing_foot_rot_interpolator.insert(std::pair(tmp_leg_types[i], new interpolator(3, dt))); // RPY + swing_foot_rot_interpolator[tmp_leg_types[i]]->setName("GaitGenerator swing_foot_rot_interpolator " + leg_type_to_leg_type_string(tmp_leg_types[i])); + std::cerr << "GaitGenerator swing_foot_rot_interpolator " + leg_type_to_leg_type_string(tmp_leg_types[i]) << std::endl;; + } + } + touch_ground_count.insert(std::pair(RLEG, 0)); + touch_ground_count.insert(std::pair(LLEG, 0)); //if (foot_ratio_interpolator == NULL) foot_ratio_interpolator = new interpolator(1, dt, interpolator::LINEAR); if (toe_heel_interpolator == NULL) toe_heel_interpolator = new interpolator(1, dt); - foot_ratio_interpolator->setName("GaitGenerator foot_ratio_interpolator"); - swing_foot_rot_ratio_interpolator->setName("GaitGenerator swing_foot_rot_ratio_interpolator"); + foot_midcoords_interpolator->setName("GaitGenerator foot_midcoords_interpolator"); toe_heel_interpolator->setName("GaitGenerator toe_heel_interpolator"); }; ~leg_coords_generator() { - if (foot_ratio_interpolator != NULL) { - delete foot_ratio_interpolator; - foot_ratio_interpolator = NULL; + if (foot_midcoords_interpolator != NULL) { + delete foot_midcoords_interpolator; + foot_midcoords_interpolator = NULL; } - if (swing_foot_rot_ratio_interpolator != NULL) { - delete swing_foot_rot_ratio_interpolator; - swing_foot_rot_ratio_interpolator = NULL; + for (std::map::iterator it = swing_foot_rot_interpolator.begin(); it != swing_foot_rot_interpolator.end(); it++) { + if (it->second != NULL) { + delete it->second; + it->second = NULL; + } } if (toe_heel_interpolator != NULL) { delete toe_heel_interpolator; toe_heel_interpolator = NULL; } - thp_ptr = NULL; }; void set_default_step_height (const double _tmp) { default_step_height = _tmp; }; void set_default_top_ratio (const double _tmp) { default_top_ratio = _tmp; }; void set_default_orbit_type (const orbit_type _tmp) { default_orbit_type = _tmp; }; + void set_is_single_walking (const size_t fnl_size) { is_single_walking = (footstep_index > 0 && footstep_index < fnl_size-2); }; void set_swing_trajectory_delay_time_offset (const double _time_offset) { sdtg.set_swing_trajectory_delay_time_offset(_time_offset); @@ -671,13 +886,24 @@ namespace rats crdtg.set_swing_trajectory_final_distance_weight(_final_distance_weight); final_distance_weight = _final_distance_weight; }; + void set_swing_trajectory_time_offset_xy2z (const double _tmp) + { + sdtg.set_swing_trajectory_time_offset_xy2z(_tmp); + cdktg.set_swing_trajectory_time_offset_xy2z(_tmp); + crdtg.set_swing_trajectory_time_offset_xy2z(_tmp); + time_offset_xy2z = _tmp; + }; void set_stair_trajectory_way_point_offset (const hrp::Vector3 _offset) { sdtg.set_stair_trajectory_way_point_offset(_offset); }; + void set_rectangle_trajectory_way_point_offset (const hrp::Vector3 _offset) { rectangle_way_point_offset = _offset; }; + void set_rectangle_goal_off (const hrp::Vector3 _offset) { rectangle_goal_off = _offset; }; + void set_rectangle_time_smooth_offset (const double _offset) { rectangle_time_smooth_offset = _offset; }; void set_cycloid_delay_kick_point_offset (const hrp::Vector3 _offset) { cdktg.set_cycloid_delay_kick_point_offset(_offset); }; void set_toe_pos_offset_x (const double _offx) { toe_pos_offset_x = _offx; }; void set_heel_pos_offset_x (const double _offx) { heel_pos_offset_x = _offx; }; void set_toe_angle (const double _angle) { toe_angle = _angle; }; void set_heel_angle (const double _angle) { heel_angle = _angle; }; void set_use_toe_joint (const bool ut) { use_toe_joint = ut; }; + void set_use_toe_heel_auto_set (const bool ut) { use_toe_heel_auto_set = ut; }; void set_swing_support_steps_list (const std::vector< std::vector >& fnsl) { std::vector prev_support_leg_steps = support_leg_steps_list.front(); @@ -706,6 +932,21 @@ namespace rats } } }; + bool set_toe_heel_phase_ratio (const std::vector& ratio) { return thp.set_toe_heel_phase_ratio(ratio); }; + void set_act_contact_states (const std::vector& _act_contact_states) { + if (act_contact_states.empty()) act_contact_states.resize(_act_contact_states.size()); + for (size_t i = 0; i < act_contact_states.size(); i++) { + act_contact_states[i] = _act_contact_states[i]; + } + }; + void set_use_act_states (const bool _use_act_states) { use_act_states = _use_act_states; }; + void set_is_stop_early_foot (const bool _is_stop_early_foot) { is_stop_early_foot = _is_stop_early_foot; }; + void set_use_force_sensor (const bool _use_force_sensor) { use_force_sensor = _use_force_sensor; }; + void set_is_early_touch (const bool _is_early_touch, const leg_type _lr) { + for (int i = 0; i < rdtg.size(); i++) { + if (swing_leg_src_steps[i].l_r == _lr) rdtg[i].is_early_touch = _is_early_touch; + } + }; void reset(const size_t _one_step_count, const size_t _next_one_step_count, const std::vector& _swing_leg_dst_steps, const std::vector& _swing_leg_src_steps, @@ -718,13 +959,17 @@ namespace rats /* swing_leg_steps.clear(); */ swing_leg_dst_steps = _swing_leg_dst_steps; swing_leg_src_steps = _swing_leg_src_steps; + current_swing_leg_steps = _swing_leg_src_steps; support_leg_steps = _support_leg_steps; support_leg_steps_list.push_back(support_leg_steps); one_step_count = lcg_count = _one_step_count; next_one_step_count = _next_one_step_count; - thp_ptr->set_one_step_count(one_step_count); + thp.set_one_step_count(one_step_count); footstep_index = 0; current_step_height = 0.0; + for (std::map::iterator it = touch_ground_count.begin(); it != touch_ground_count.end(); it++) { + it->second = 0; + } switch (default_orbit_type) { case RECTANGLE: rdtg.clear(); @@ -732,7 +977,7 @@ namespace rats rdtg.push_back(rectangle_delay_hoffarbib_trajectory_generator()); rdtg.back().reset_all(dt, one_step_count, default_double_support_ratio_before, default_double_support_ratio_after, - time_offset, final_distance_weight); + time_offset, final_distance_weight, time_offset_xy2z); } break; case STAIR: @@ -744,7 +989,7 @@ namespace rats cdtg.push_back(cycloid_delay_hoffarbib_trajectory_generator()); cdtg.back().reset_all(dt, one_step_count, default_double_support_ratio_before, default_double_support_ratio_after, - time_offset, final_distance_weight); + time_offset, final_distance_weight, time_offset_xy2z); } break; case CYCLOIDDELAYKICK: @@ -756,34 +1001,70 @@ namespace rats default: break; } - reset_foot_ratio_interpolator(); - }; - void reset_foot_ratio_interpolator () - { - double tmp_ratio = 0.0; - foot_ratio_interpolator->clear(); - foot_ratio_interpolator->set(&tmp_ratio); - tmp_ratio = 1.0; - //foot_ratio_interpolator->go(&tmp_ratio, dt*one_step_count, true); - foot_ratio_interpolator->setGoal(&tmp_ratio, dt*one_step_count, true); - foot_ratio_interpolator->sync(); + current_src_toe_heel_type = current_dst_toe_heel_type = SOLE; }; void clear_interpolators ( ) { - double tmp; - while (!swing_foot_rot_ratio_interpolator->isEmpty()) { - swing_foot_rot_ratio_interpolator->get(&tmp, true); + double tmpsw[3]; + for (std::map::iterator it = swing_foot_rot_interpolator.begin(); it != swing_foot_rot_interpolator.end(); it++) { + while (!it->second->isEmpty()) it->second->get(tmpsw, true); } - while (!foot_ratio_interpolator->isEmpty()) { - foot_ratio_interpolator->get(&tmp, true); + double tmpfm[foot_midcoords_interpolator->dimension()]; + while (!foot_midcoords_interpolator->isEmpty()) { + foot_midcoords_interpolator->get(tmpfm, true); } + double tmp; while (!toe_heel_interpolator->isEmpty()) { toe_heel_interpolator->get(&tmp, true); } }; bool is_same_footstep_nodes(const std::vector& fns_1, const std::vector& fns_2) const; - void update_leg_steps (const std::vector< std::vector >& fnsl, const double default_double_support_ratio_before, const double default_double_support_ratio_after); + void update_leg_steps (const std::vector< std::vector >& fnsl, const double default_double_support_ratio_before, const double default_double_support_ratio_after, const toe_heel_type_checker& thtc); + void calc_swing_leg_src_steps (std::vector& ret_swing_leg_src_steps, const std::vector< std::vector >& fnsl, const size_t _footstep_index) + { + if (_footstep_index > 0) { + if (is_same_footstep_nodes(fnsl[_footstep_index], fnsl[_footstep_index-1])) { + ret_swing_leg_src_steps = swing_leg_dst_steps_list[_footstep_index-1]; + } else { + /* current swing leg src coords = (previout support leg coords + previous swing leg dst coords) - current support leg coords */ + std::vector tmp_swing_leg_src_steps = support_leg_steps_list[_footstep_index-1]; + std::copy(swing_leg_dst_steps_list[_footstep_index-1].begin(), + swing_leg_dst_steps_list[_footstep_index-1].end(), + std::back_inserter(tmp_swing_leg_src_steps)); + std::vector tmp_support_leg_steps = support_leg_steps_list[_footstep_index]; + for (size_t i = 0; i < tmp_support_leg_steps.size(); i++) { + std::vector::iterator it = std::remove_if(tmp_swing_leg_src_steps.begin(), tmp_swing_leg_src_steps.end(), (&boost::lambda::_1->* &step_node::l_r == tmp_support_leg_steps.at(i).l_r)); + tmp_swing_leg_src_steps.erase(it, tmp_swing_leg_src_steps.end()); + } + ret_swing_leg_src_steps = tmp_swing_leg_src_steps; + } + } + }; + void calc_swing_support_params_from_footstep_nodes_list (const std::vector< std::vector >& fnsl) + { + // Get current swing coords, support coords, and support leg parameters + size_t current_footstep_index = (footstep_index < fnsl.size() - 1 ? footstep_index : fnsl.size()-1); + swing_leg_dst_steps = fnsl[current_footstep_index]; + if (footstep_index != 0) { // If not initial step, support_leg_coords is previous swing_leg_dst_coords // why we need this? + support_leg_steps = support_leg_steps_list[current_footstep_index]; + } + support_leg_types.clear(); + for (std::vector::iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) { + support_leg_types.push_back(it->l_r); + } + swing_leg_types.clear(); + for (std::vector::iterator it = swing_leg_dst_steps.begin(); it != swing_leg_dst_steps.end(); it++) { + swing_leg_types.push_back(it->l_r); + } + calc_swing_leg_src_steps(swing_leg_src_steps, fnsl, current_footstep_index); + }; size_t get_footstep_index() const { return footstep_index; }; size_t get_lcg_count() const { return lcg_count; }; + void set_lcg_count (const size_t cnt) { lcg_count = cnt; }; + void set_one_step_count (const size_t d_cnt) {one_step_count += d_cnt; }; + void reset_one_step_count (const size_t d_cnt, const double before, const double after) { + for (size_t i = 0; i < rdtg.size(); i++) + rdtg[i].reset_one_step_count(d_cnt, before, after); + }; double get_current_swing_time(const size_t idx) const { return current_swing_time.at(idx); }; const std::vector& get_swing_leg_steps() const { return swing_leg_steps; }; const std::vector& get_support_leg_steps() const { return support_leg_steps; }; @@ -796,21 +1077,7 @@ namespace rats double get_default_step_height () const { return default_step_height;}; void get_swing_support_mid_coords(coordinates& ret) const { - std::vector swg_coords, sup_coords; - for (std::vector::const_iterator it_src = swing_leg_src_steps.begin(), it_dst = swing_leg_dst_steps.begin(); - it_src != swing_leg_src_steps.end() && it_dst != swing_leg_dst_steps.end(); - it_src++, it_dst++) { - coordinates tmp; - mid_coords(tmp, foot_midcoords_ratio, it_src->worldcoords, it_dst->worldcoords); - if (it_src->l_r == RLEG or it_src->l_r == LLEG) swg_coords.push_back(tmp); - } - for (std::vector::const_iterator it = support_leg_steps.begin(); it != support_leg_steps.end(); it++) { - if (it->l_r == RLEG or it->l_r == LLEG) sup_coords.push_back(it->worldcoords); - } - coordinates tmp_swg_mid, tmp_sup_mid; - if (swg_coords.size() > 0) multi_mid_coords(tmp_swg_mid, swg_coords); - if (sup_coords.size() > 0) multi_mid_coords(tmp_sup_mid, sup_coords); - mid_coords(ret, static_cast(sup_coords.size()) / (swg_coords.size() + sup_coords.size()), tmp_swg_mid, tmp_sup_mid); + ret = swing_support_midcoords; }; std::vector get_current_support_states () const { @@ -826,7 +1093,11 @@ namespace rats orbit_type get_default_orbit_type () const { return default_orbit_type; }; double get_swing_trajectory_delay_time_offset () const { return time_offset; }; double get_swing_trajectory_final_distance_weight () const { return final_distance_weight; }; + double get_swing_trajectory_time_offset_xy2z () const { return time_offset_xy2z; }; hrp::Vector3 get_stair_trajectory_way_point_offset () const { return sdtg.get_stair_trajectory_way_point_offset(); }; + hrp::Vector3 get_rectangle_trajectory_way_point_offset () const { return rectangle_way_point_offset; }; + hrp::Vector3 get_rectangle_goal_off () const { return rectangle_goal_off; }; + double get_rectangle_time_smooth_offset () const { return rectangle_time_smooth_offset; }; hrp::Vector3 get_cycloid_delay_kick_point_offset () const { return cdktg.get_cycloid_delay_kick_point_offset() ; }; double get_toe_pos_offset_x () const { return toe_pos_offset_x; }; double get_heel_pos_offset_x () const { return heel_pos_offset_x; }; @@ -834,18 +1105,49 @@ namespace rats double get_heel_angle () const { return heel_angle; }; double get_foot_dif_rot_angle () const { return foot_dif_rot_angle; }; bool get_use_toe_joint () const { return use_toe_joint; }; + bool get_use_toe_heel_auto_set () const { return use_toe_heel_auto_set; }; + void get_toe_heel_phase_ratio (std::vector& ratio) const { thp.get_toe_heel_phase_ratio(ratio); }; + double get_current_toe_heel_ratio (const bool _use_toe_heel_transition) const + { + if (_use_toe_heel_transition && current_step_height > 0.0) { // If swing phase + return thp.calc_phase_ratio_for_toe_heel_transition(one_step_count - lcg_count); + } else { // If support phase such as double support phase of starting and ending. + return no_using_toe_heel_ratio; + } + }; +#ifdef FOR_TESTGAITGENERATOR + size_t get_one_step_count() const { return one_step_count; }; + double get_toe_heel_dif_angle() const { return toe_heel_dif_angle; }; +#endif // FOR_TESTGAITGENERATOR }; class gait_generator { public: + bool use_act_states, is_inverse_double_phase, is_emergency_touch_wall, is_stuck, is_interpolate_zmp_in_double, is_stable_go_stop_mode, use_flywheel_balance; + double zmp_delay_time_const, overwritable_max_time, fg_zmp_cutoff_freq; + hrp::Vector3 fxy; + boost::shared_ptr > cp_filter; + double debug_orig_height, debug_landing_height_xrange[2], debug_landing_height; + bool debug_set_landing_height; + Eigen::Vector2d front_edge_offset_of_steppable_region; + bool is_slow_stair_mode; + double stair_step_time; + double footguided_balance_time_const; + size_t num_preview_step; + double total_external_force_z; + bool is_jumping; + double debug_steppable_height; + bool is_wheeling; #ifndef HAVE_MAIN private: #endif enum velocity_mode_flag { VEL_IDLING, VEL_DOING, VEL_ENDING }; enum emergency_flag { IDLING, EMERGENCY_STOP, STOPPING }; + enum projected_point_region {LEFT, MIDDLE, RIGHT}; + enum JumpPhase { BEFORE_JUMP, JUMPING, AFTER_JUMP }; /* member variables for gait_generator */ // Footstep list to be executed @@ -853,17 +1155,17 @@ namespace rats std::vector< std::vector > footstep_nodes_list; // Footstep list for overwriting future footstep queue std::vector< std::vector > overwrite_footstep_nodes_list; - toe_heel_phase_counter thp; refzmp_generator rg; leg_coords_generator lcg; footstep_parameter footstep_param; velocity_mode_parameter vel_param, offset_vel_param; - hrp::Vector3 cog, refzmp, prev_que_rzmp; /* cog by calculating proc_one_tick */ + toe_heel_type_checker thtc; + hrp::Vector3 cog, refzmp, prev_que_rzmp, zmp; /* cog by calculating proc_one_tick */ std::vector swing_foot_zmp_offsets, prev_que_sfzos; double dt; /* control loop [s] */ std::vector all_limbs; - double default_step_time; - double default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after; + double default_step_time, orig_default_step_time; + double default_double_support_ratio_before, default_double_support_ratio_after, default_double_support_static_ratio_before, default_double_support_static_ratio_after, orig_default_double_support_static_ratio_before, orig_default_double_support_static_ratio_after; double default_double_support_ratio_swing_before; /*first double support time for leg coords generator */ double default_double_support_ratio_swing_after; /*last double support time for leg coords generator */ double gravitational_acceleration; @@ -879,11 +1181,58 @@ namespace rats bool use_inside_step_limitation; std::map leg_type_map; coordinates initial_foot_mid_coords; - bool solved; + bool solved, is_first_count, is_preview, is_set_first_count, is_first_double_after, is_double_support_phase, is_after_double_support_phase, was_enlarged_time; + size_t remain_count; + size_t touchoff_remain_count[2]; // assume biped robot + double leg_margin[4], safe_leg_margin[4], stride_limitation_for_circle_type[5], overwritable_stride_limitation[5], footstep_modification_gain, cp_check_margin[2], margin_time_ratio; + bool use_stride_limitation, is_emergency_walking[2], modify_footsteps, is_emergency_step; + hrp::Vector3 diff_cp, modified_d_footstep, sum_d_footstep_plus, sum_d_footstep_minus, sum_d_footstep_thre, footstep_hist_max, footstep_hist_min, footstep_check_delta; + double modified_d_step_time, min_time_mgn, min_time, orig_min_time, emergency_step_time[3]; + std::vector act_contact_states; + stride_limitation_type default_stride_limitation_type; + double act_vel_ratio, double_remain_count_offset, fg_double_remain_count; + hrp::Vector3 fg_ref_zmp, prev_fg_ref_zmp, fg_start_ref_zmp, prev_start_ref_zmp, fg_goal_ref_zmp, prev_ref_dcm, flywheel_tau, prev_short_of_zmp, ref_cp, act_cp, prev_act_cp, sum_fx, sum_fy, des_fxy, ref_footstep_offset; + bool updated_vel_footsteps, use_roll_flywheel, use_pitch_flywheel, use_disturbance_compensation, prev_use_roll_flywheel, prev_use_pitch_flywheel; + std::vector > foot_vertices; + std::vector convex_hull; + size_t fg_step_count, falling_direction; + double total_mass, dc_gain, dcm_offset; + double tmp[23]; + hrp::Vector3 rel_landing_pos, end_cog, end_cogvel; + int cur_supporting_foot, fx_count, fy_count; + bool is_vision_updated, lr_region[2], was_read_steppable_height; + bool changed_step_time_stair; + std::vector stride_limitation_polygon; + hrp::Vector3 dc_foot_rpy, dc_landing_pos, orig_current_foot_rpy, vel_foot_offset, rel_landing_height, rel_landing_normal; + std::vector > > steppable_region; + std::vector > steppable_height; + std::vector > > debug_current_steppable_region; + boost::shared_ptr > fx_filter; + boost::shared_ptr > zmp_filter; + boost::shared_ptr double_support_zmp_interpolator; + StepNumPhase step_num_phase; + WalkingPhase walking_phase; + boost::shared_ptr wheel_interpolator; + + coordinates initial_jump_midcoords, jump_midcoords; + step_node initial_support_leg, initial_swing_leg; + double jump_remain_time, jump_landing_height, jump_takeoff_height, jump_flight_time, jump_recover_time; + hrp::Vector3 jump_last_cp, d_jump_foot_pos, initial_jump_cog, d_jump_pos; + JumpPhase jump_phase; + boost::shared_ptr jump_foot_interpolator; + + // wheel + std::vector< std::vector > wheel_nodes_list; + int wheel_major_index, wheel_index; + double cur_wheel_ratio, cur_wheel_pos_x, start_wheel_pos_x; + coordinates wheel_midcoords, initial_wheel_midcoords; + hrp::Vector3 d_wheel_pos, final_footstep_pos; + bool is_enlarged_final_time_for_wheel; /* preview controller parameters */ //preview_dynamics_filter* preview_controller_ptr; preview_dynamics_filter* preview_controller_ptr; + foot_guided_controller<3>* foot_guided_controller_ptr; void append_go_pos_step_nodes (const coordinates& _ref_coords, const std::vector& lts) @@ -900,15 +1249,20 @@ namespace rats sns.push_back(step_node(lts.at(i), _ref_coords, lcg.get_default_step_height(), default_step_time, lcg.get_toe_angle(), lcg.get_heel_angle())); - sns.at(i).worldcoords.pos += sns.at(i).worldcoords.rot * footstep_param.leg_default_translate_pos[lts.at(i)]; + // sns.at(i).worldcoords.pos += sns.at(i).worldcoords.rot * footstep_param.leg_default_translate_pos[lts.at(i)]; + sns.at(i).worldcoords.pos += sns.at(i).worldcoords.rot * footstep_param.vel_foot_offset[lts.at(i)]; } _footstep_nodes_list.push_back(sns); }; - void overwrite_refzmp_queue(const std::vector< std::vector >& fnsl); + void overwrite_refzmp_queue(const std::vector< std::vector >& fnsl, const hrp::Vector3& cur_cog = hrp::Vector3::Zero(), const hrp::Vector3& cur_cogvel = hrp::Vector3::Zero(), const hrp::Vector3& cur_refcog = hrp::Vector3::Zero(), const hrp::Vector3& cur_refcogvel = hrp::Vector3::Zero(), const hrp::Vector3& target_cog = hrp::Vector3::Zero()); void calc_ref_coords_trans_vector_velocity_mode (coordinates& ref_coords, hrp::Vector3& trans, double& dth, const std::vector& sup_fns, const velocity_mode_parameter& cur_vel_param) const; void calc_next_coords_velocity_mode (std::vector< std::vector >& ret_list, const size_t idx, const size_t future_step_num = 3); void append_footstep_list_velocity_mode (); void append_footstep_list_velocity_mode (std::vector< std::vector >& _footstep_nodes_list, const velocity_mode_parameter& cur_vel_param) const; + inline leg_type get_leg_type_from_ee_name (const std::string& ee_name) const + { + return std::find_if(leg_type_map.begin(), leg_type_map.end(), (&boost::lambda::_1->* &std::map::value_type::second == ee_name))->first; + }; #ifndef HAVE_MAIN /* inhibit copy constructor and copy insertion not by implementing */ @@ -919,30 +1273,84 @@ namespace rats gait_generator (double _dt, /* arguments for footstep_parameter */ const std::vector& _leg_pos, std::vector _all_limbs, - const double _stride_fwd_x, const double _stride_y, const double _stride_theta, const double _stride_bwd_x) - : footstep_nodes_list(), overwrite_footstep_nodes_list(), thp(), rg(&thp, _dt), lcg(_dt, &thp), all_limbs(_all_limbs), - footstep_param(_leg_pos, _stride_fwd_x, _stride_y, _stride_theta, _stride_bwd_x), - vel_param(), offset_vel_param(), cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()), - dt(_dt), default_step_time(1.0), default_double_support_ratio_before(0.1), default_double_support_ratio_after(0.1), default_double_support_static_ratio_before(0.0), default_double_support_static_ratio_after(0.0), default_double_support_ratio_swing_before(0.1), default_double_support_ratio_swing_after(0.1), gravitational_acceleration(DEFAULT_GRAVITATIONAL_ACCELERATION), - finalize_count(0), optional_go_pos_finalize_footstep_num(0), overwrite_footstep_index(0), overwritable_footstep_index_offset(1), - velocity_mode_flg(VEL_IDLING), emergency_flg(IDLING), - use_inside_step_limitation(true), - preview_controller_ptr(NULL) { - swing_foot_zmp_offsets = boost::assign::list_of(hrp::Vector3::Zero()); - prev_que_sfzos = boost::assign::list_of(hrp::Vector3::Zero()); - leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm"); + const double _stride_fwd_x, const double _stride_outside_y, const double _stride_outside_theta, + const double _stride_bwd_x, const double _stride_inside_y, const double _stride_inside_theta) + : footstep_nodes_list(), overwrite_footstep_nodes_list(), rg(_dt), lcg(_dt), + footstep_param(_leg_pos, _stride_fwd_x, _stride_outside_y, _stride_outside_theta, _stride_bwd_x, _stride_inside_y, _stride_inside_theta), + vel_param(), offset_vel_param(), thtc(), cog(hrp::Vector3::Zero()), refzmp(hrp::Vector3::Zero()), prev_que_rzmp(hrp::Vector3::Zero()), diff_cp(hrp::Vector3::Zero()), modified_d_footstep(hrp::Vector3::Zero()), zmp(hrp::Vector3::Zero()), modified_d_step_time(0.0), + dt(_dt), all_limbs(_all_limbs), default_step_time(1.0), default_double_support_ratio_before(0.1), default_double_support_ratio_after(0.1), default_double_support_static_ratio_before(0.0), default_double_support_static_ratio_after(0.0), default_double_support_ratio_swing_before(0.1), default_double_support_ratio_swing_after(0.1), gravitational_acceleration(DEFAULT_GRAVITATIONAL_ACCELERATION), + finalize_count(0), optional_go_pos_finalize_footstep_num(0), overwrite_footstep_index(0), overwritable_footstep_index_offset(1), is_emergency_step(false), + velocity_mode_flg(VEL_IDLING), emergency_flg(IDLING), margin_time_ratio(0.01), footstep_modification_gain(5e-6), act_vel_ratio(1.0), use_disturbance_compensation(false), dc_gain(1e-4), num_preview_step(2), + use_inside_step_limitation(true), use_stride_limitation(false), modify_footsteps(false), default_stride_limitation_type(SQUARE), is_first_count(false), is_first_double_after(true), double_remain_count_offset(0.0), use_roll_flywheel(false), use_pitch_flywheel(false), dcm_offset(0.0), rel_landing_pos(hrp::Vector3::Zero()), cur_supporting_foot(0), is_vision_updated(false), was_read_steppable_height(false), rel_landing_height(hrp::Vector3::Zero()), rel_landing_normal(hrp::Vector3::UnitZ()), zmp_delay_time_const(0.0), is_inverse_double_phase(false), overwritable_max_time(2.0), fg_zmp_cutoff_freq(1e6), is_emergency_touch_wall(false), end_cog(hrp::Vector3::Zero()), end_cogvel(hrp::Vector3::Zero()), sum_d_footstep_plus(hrp::Vector3::Zero()), sum_d_footstep_minus(hrp::Vector3::Zero()), footstep_hist_max(hrp::Vector3::Zero()), footstep_hist_min(hrp::Vector3::Zero()), is_stuck(false), is_interpolate_zmp_in_double(true), is_stable_go_stop_mode(false), use_flywheel_balance(false), stair_step_time(0.5), footguided_balance_time_const(0.4), is_slow_stair_mode(false), changed_step_time_stair(false), is_jumping(false), debug_steppable_height(0.0), is_wheeling(false), + preview_controller_ptr(NULL), foot_guided_controller_ptr(NULL), is_preview(false), updated_vel_footsteps(false), min_time_mgn(0.2), min_time(0.5), flywheel_tau(hrp::Vector3::Zero()), falling_direction(0), dc_foot_rpy(hrp::Vector3::Zero()), dc_landing_pos(hrp::Vector3::Zero()), sum_fx(hrp::Vector3::Zero()), sum_fy(hrp::Vector3::Zero()), des_fxy(hrp::Vector3::Zero()), fx_count(0), fy_count(0), debug_set_landing_height(false), debug_landing_height(0.0), front_edge_offset_of_steppable_region(Eigen::Vector2d::Zero()) { + swing_foot_zmp_offsets.assign (1, hrp::Vector3::Zero()); + prev_que_sfzos.assign (1, hrp::Vector3::Zero()); + leg_type_map = boost::assign::map_list_of(RLEG, "rleg")(LLEG, "lleg")(RARM, "rarm")(LARM, "larm").convert_to_container < std::map > (); + stride_limitation_polygon.resize(4); + steppable_region.resize(2); + steppable_height.resize(2); + debug_current_steppable_region.resize(2); + for (size_t i = 0; i < 4; i++) leg_margin[i] = 0.1; + for (size_t i = 0; i < 4; i++) safe_leg_margin[i] = 0.1; + for (size_t i = 0; i < 5; i++) stride_limitation_for_circle_type[i] = 0.2; + for (size_t i = 0; i < 5; i++) overwritable_stride_limitation[i] = 0.2; + for (size_t i = 0; i < 2; i++) is_emergency_walking[i] = false; + for (size_t i = 0; i < 2; i++) cp_check_margin[i] = 0.025; + for (size_t i = 0; i < 3; i++) emergency_step_time[i] = 0.8; + for (size_t i = 0; i < 2; i++) debug_landing_height_xrange[i] = 0.0; + for (size_t i = 0; i < 23; i++) tmp[i] = 0.0; + sum_d_footstep_thre = hrp::Vector3(0.5, 0.5, 0.0); + footstep_check_delta = hrp::Vector3(0.03, 0.03, 0.0); + cp_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(1e6, _dt, hrp::Vector3::Zero())); + fx_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(0.1, _dt, hrp::Vector3::Zero())); + zmp_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(4.0, _dt, hrp::Vector3::Zero())); + double_support_zmp_interpolator = boost::shared_ptr(new interpolator(1, dt, interpolator::HOFFARBIB)); + double_support_zmp_interpolator->setName("GaitGenerator double_support_zmp_interpolator"); + jump_foot_interpolator = boost::shared_ptr(new interpolator(3, dt, interpolator::HOFFARBIB)); + jump_foot_interpolator->setName("GaitGenerator jump_foot_interpolator"); + wheel_interpolator = boost::shared_ptr(new interpolator(1, dt, interpolator::LINEAR)); + wheel_interpolator->setName("GaitGenerator wheel_interpolator"); }; ~gait_generator () { if ( preview_controller_ptr != NULL ) { delete preview_controller_ptr; preview_controller_ptr = NULL; } + if ( foot_guided_controller_ptr != NULL ) { + delete foot_guided_controller_ptr; + foot_guided_controller_ptr = NULL; + } }; - void initialize_gait_parameter (const hrp::Vector3& cog, + void initialize_gait_parameter (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_refcog, const std::vector& initial_support_leg_steps, const std::vector& initial_swing_leg_dst_steps, const double delay = 1.6); - bool proc_one_tick (); + void initialize_jump_parameter (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_refcog, + const std::vector& initial_support_leg_steps, + const std::vector& initial_swing_leg_dst_steps, + const coordinates& start_ref_coords, + const hrp::Vector3& trans, + const double t_squat, const double t_flight); + void initialize_wheel_parameter (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_refcog, + const std::vector& initial_support_leg_steps, + const std::vector& initial_swing_leg_dst_steps); + bool proc_one_tick (hrp::Vector3 cur_cog = hrp::Vector3::Zero(), hrp::Vector3 cur_cogvel = hrp::Vector3::Zero(), const hrp::Vector3& target_cog = hrp::Vector3::Zero()); + bool proc_one_tick_jump (hrp::Vector3 cur_cog = hrp::Vector3::Zero(), const hrp::Vector3& cur_cogvel = hrp::Vector3::Zero(), const hrp::Vector3& target_cog = hrp::Vector3::Zero()); + bool proc_one_tick_wheel (hrp::Vector3 cur_cog = hrp::Vector3::Zero(), hrp::Vector3 cur_cogvel = hrp::Vector3::Zero(), const hrp::Vector3& target_cog = hrp::Vector3::Zero()); + void project_to_nominal_ground (hrp::Vector3& p, const hrp::Vector3 ground, const hrp::Vector3 cog); + void update_preview_controller(bool& solved); + void update_foot_guided_controller(bool& solved, const hrp::Vector3& cur_cog, const hrp::Vector3& cur_cogvel, const hrp::Vector3& cur_refcog, const hrp::Vector3& cur_refcogvel, const hrp::Vector3& target_cog); + bool update_wheel_controller(); + void calc_last_cp(hrp::Vector3& last_cp, const coordinates& cur_step); + void calc_foot_origin_rot (hrp::Matrix33& foot_rot, const hrp::Matrix33& orig_rot, const hrp::Vector3& n) const; + void set_first_count_flag (); + void min_time_check (double& tmp_dt); + void change_step_time (const double& tmp_dt); + void limit_stride (step_node& cur_fs, const step_node& prev_fs, const double (&limit)[5]) const; + void limit_stride_rectangle (step_node& cur_fs, const step_node& prev_fs, const double (&limit)[5]) const; + void limit_stride_vision (step_node& cur_fs, hrp::Vector3& short_of_footstep, const step_node& prev_fs, const step_node& preprev_fs, const double& omega, const hrp::Vector3& cur_cp); + void modify_footsteps_for_recovery (); + void modify_footsteps_for_foot_guided (const hrp::Vector3& cur_cog, const hrp::Vector3& cur_cogvel, const hrp::Vector3& cur_refcog, const hrp::Vector3& cur_refcogvel, const hrp::Vector3& target_cog); void append_footstep_nodes (const std::vector& _legs, const std::vector& _fss) { std::vector tmp_sns; @@ -967,31 +1375,35 @@ namespace rats bool go_pos_param_2_footstep_nodes_list (const double goal_x, const double goal_y, const double goal_theta, /* [mm] [mm] [deg] */ const std::vector& initial_support_legs_coords, coordinates start_ref_coords, const std::vector& initial_support_legs, - const bool is_initialize = true) { + const bool is_initialize = true, + const double tm_off = 0.0) { std::vector< std::vector > new_footstep_nodes_list; return go_pos_param_2_footstep_nodes_list (goal_x, goal_y, goal_theta, initial_support_legs_coords, start_ref_coords, initial_support_legs, new_footstep_nodes_list, - is_initialize); + is_initialize, tm_off); }; bool go_pos_param_2_footstep_nodes_list (const double goal_x, const double goal_y, const double goal_theta, /* [mm] [mm] [deg] */ const std::vector& initial_support_legs_coords, coordinates start_ref_coords, const std::vector& initial_support_legs, std::vector< std::vector >& new_footstep_nodes_list, - const bool is_initialize = true); + const bool is_initialize = true, const double tm_off = 0.0); void go_pos_param_2_footstep_nodes_list_core (const double goal_x, const double goal_y, const double goal_theta, /* [mm] [mm] [deg] */ const std::vector& initial_support_legs_coords, coordinates start_ref_coords, const std::vector& initial_support_legs, std::vector< std::vector >& new_footstep_nodes_list, - const bool is_initialize, const size_t overwritable_fs_index) const; + const bool is_initialize, const size_t overwritable_fs_index, + const double tm_off = 0.0) const; void go_single_step_param_2_footstep_nodes_list (const double goal_x, const double goal_y, const double goal_z, const double goal_theta, /* [mm] [mm] [mm] [deg] */ const std::string& tmp_swing_leg, const coordinates& _support_leg_coords); + bool go_wheel_param_2_wheel_nodes_list (const std::vector& goal_x_list, const std::vector& v_max_list, const std::vector& a_max_list, const coordinates& start_ref_coords, const bool with_footstep); void initialize_velocity_mode (const coordinates& _ref_coords, const double vel_x, const double vel_y, const double vel_theta, /* [mm/s] [mm/s] [deg/s] */ const std::vector& current_legs); void finalize_velocity_mode (); + void finalize_velocity_mode2 (); void append_finalize_footstep () { append_finalize_footstep(footstep_nodes_list); @@ -1011,6 +1423,295 @@ namespace rats emergency_flg = EMERGENCY_STOP; } }; + double calcCrossProduct(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& o = Eigen::Vector2d::Zero()) + { + return (a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0)); + }; + inline projected_point_region calcProjectedPoint(Eigen::Vector2d& ret, const Eigen::Vector2d& target, const Eigen::Vector2d& a, const Eigen::Vector2d& b) + { + Eigen::Vector2d v1 = target - b; + Eigen::Vector2d v2 = a - b; + double v2_norm = v2.norm(); + if ( v2_norm == 0 ) { + ret = a; + return LEFT; + } else { + double ratio = v1.dot(v2) / (v2_norm * v2_norm); + if (ratio < 0){ + ret = b; + return RIGHT; + } else if (ratio > 1){ + ret = a; + return LEFT; + } else { + ret = b + ratio * v2; + return MIDDLE; + } + } + }; + // Assume that the oder of ch is CCW or CW + inline bool is_inside_polygon (Eigen::Vector2d& p, const std::vector& ch, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false) + { + bool is_inside = false; + for (int i = 0; i < ch.size(); i++) { + Eigen::Vector2d a = ch[i] - p, b = ch[(i+1)%ch.size()] - p; + if (a(1) > b(1)) swap(a, b); + if (a(1) <= 0 && 0 < b(1)) + if (calcCrossProduct(a, b) < 0) is_inside = !is_inside; + // if (calcCrossProduct(a, b) == 0 && a.dot(b) <= 0) return true; // On edge + } + return is_inside; + } + inline bool is_inside_convex_hull (Eigen::Vector2d& p, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false) + { + double tmp; + hrp::Vector3 tmp_pos; + return is_inside_convex_hull(p, tmp, tmp_pos, convex_hull, offset, truncate_p); + }; + inline bool is_inside_convex_hull (Eigen::Vector2d& p, const std::vector& ch, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false) + { + double tmp; + hrp::Vector3 tmp_pos; + return is_inside_convex_hull(p, tmp, tmp_pos, ch, offset, truncate_p); + }; + inline bool is_inside_convex_hull (Eigen::Vector2d& p, double& t, hrp::Vector3& short_of_footstep, const std::vector& ch, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false, const bool& change_time = false, const double& r = 0.0, const double& omega = 0.0, const hrp::Vector3& cur_cp = hrp::Vector3::Zero(), const hrp::Vector3& prev_fs_pos = hrp::Vector3::Zero(), const hrp::Matrix33& prev_fs_rot = hrp::Matrix33::Ones(), const leg_type cur_sup = RLEG) + { + // set any inner point(ip) and binary search two vertices(ch[v_a], ch[v_b]) between which p is. + p -= offset.head(2); + size_t n_ch = ch.size(); + Eigen::Vector2d ip = (ch[0] + ch[n_ch/3] + ch[2*n_ch/3]) / 3.0; + size_t v_a = 0, v_b = n_ch; + while (v_a + 1 < v_b) { + size_t v_c = (v_a + v_b) / 2; + if (calcCrossProduct(ch[v_a], ch[v_c], ip) > 0) { + if (calcCrossProduct(ch[v_a], p, ip) > 0 && calcCrossProduct(ch[v_c], p, ip) < 0) v_b = v_c; + else v_a = v_c; + } else { + if (calcCrossProduct(ch[v_a], p, ip) < 0 && calcCrossProduct(ch[v_c], p, ip) > 0) v_a = v_c; + else v_b = v_c; + } + } + v_b %= n_ch; + if (calcCrossProduct(ch[v_a], ch[v_b], p) >= 0) { + p += offset.head(2); + return true; + } else { + if (truncate_p) { + if (!calc_closest_boundary_point(p, ch, v_a, v_b)) std::cerr << "Cannot calculate closest boundary point on the convex hull" << std::endl; + } else if (change_time) { // TODO: should consider dcm_off/zmp_offset + // prev foot frame + Eigen::Vector2d pa = (prev_fs_rot.transpose() * (hrp::Vector3(ch[v_a](0), ch[v_a](1), 0) - prev_fs_pos)).head(2), + pb = (prev_fs_rot.transpose() * (hrp::Vector3(ch[v_b](0), ch[v_b](1), 0) - prev_fs_pos)).head(2), + cp = (prev_fs_rot.transpose() * (cur_cp - prev_fs_pos)).head(2); + p = (prev_fs_rot.transpose() * (hrp::Vector3(p(0), p(1), 0) - prev_fs_pos)).head(2); + double tmp = fabs(pb(0) - pa(0)) < 1e-3 ? 1e-3 : (pb(0) - pa(0)); // limit 1[mm] + double a = (pb(1) - pa(1)) / tmp, b = pa(1) - pa(0) * (pb(1) - pa(1)) / tmp; + double ar = (a*a+1)*r*r; + double A = ar - std::pow(a*cp(0)-cp(1),2.0); + double B = -2 * (a*b*cp(0)-b*cp(1)+ar); + double C = ar - b*b; + double ewt = std::exp(omega*t); + double D = A * ewt * ewt + B * ewt + C; + double tmp_dt = 0.0; + if (D >= 0) { + Eigen::Vector2d tmp_pos = ewt * cp; + // preprev foot frame + tmp_pos = (prev_fs_pos + prev_fs_rot * hrp::Vector3(tmp_pos(0), tmp_pos(1), 0)).head(2); + if (!calc_closest_boundary_point(tmp_pos, ch, v_a, v_b)) std::cerr << "Cannot calculate closest boundary point on the convex hull" << std::endl; + p = tmp_pos; + } else { + D = B*B - 4*A*C; + if (D >= 0) { + double tmp_t = std::log((-B + std::sqrt(D)) / (2*A)) / omega; + if (isnan(tmp_t)) tmp_t = std::log((-B - std::sqrt(D)) / (2*A)) / omega; + if (isfinite(tmp_t)) { // limit projected point onto nearest edge by rectangular capture region(RCR) + // projecte point onto nearest edge + p(0) = (a * (p(1) - b) + p(0)) / (a*a + 1); + p(1) = a * p(0) + b; + + double end_cp_front = std::exp(omega * tmp_t) * cp(0) - (std::exp(omega * tmp_t) - 1) * safe_leg_margin[0]; + double end_cp_back = std::exp(omega * tmp_t) * cp(0) + (std::exp(omega * tmp_t) - 1) * safe_leg_margin[1]; + double end_cp_outside = std::exp(omega * tmp_t) * cp(1) - (cur_sup == RLEG ? -1 : 1) * (std::exp(omega * tmp_t) - 1) * safe_leg_margin[2]; + double end_cp_inside = std::exp(omega * tmp_t) * cp(1) - (cur_sup == RLEG ? 1 : -1) * (std::exp(omega * tmp_t) - 1) * safe_leg_margin[3]; + // < C++11 + // std::vector rcr = {Eigen::Vector2d(end_cp_front, end_cp_outside), + // Eigen::Vector2d(end_cp_back, end_cp_outside), + // Eigen::Vector2d(end_cp_back, end_cp_inside), + // Eigen::Vector2d(end_cp_front, end_cp_inside)}; + // C++98 + std::vector rcr(4); + rcr[0] = Eigen::Vector2d(end_cp_front, end_cp_outside); + rcr[1] = Eigen::Vector2d(end_cp_back, end_cp_outside); + rcr[2] = Eigen::Vector2d(end_cp_back, end_cp_inside); + rcr[3] = Eigen::Vector2d(end_cp_front, end_cp_inside); + std::vector s_cand(4); // intersection + for (size_t i = 0; i < rcr.size(); i++) { + double tmp = fabs(pb(0) - pa(0)) < 1e-3 ? 1e-3 : (pb(0) - pa(0)); // limit 1[mm] + double rcr_a = (rcr[(i+1)%4](1) - rcr[i](1)) / tmp, rcr_b = rcr[i](1) - rcr[i](0) * (rcr[(i+1)%4](1) - rcr[i](1)) / tmp; + tmp = fabs(a - rcr_a) < 1e-3 ? 1e-3 : (a - rcr_a); // limit 1[mm] + s_cand[i](0) = (rcr_b - b) / tmp; + s_cand[i](1) = (a * rcr_b - rcr_a * b) / tmp; + } + std::vector s; + s.reserve(2); + // vertical check + if (s_cand[0](0) < end_cp_front && s_cand[0](0) > end_cp_back) s.push_back(s_cand[0]); + if (s_cand[2](0) < end_cp_front && s_cand[2](0) > end_cp_back) s.push_back(s_cand[2]); + // horizontal check + if ((s_cand[1](1) < end_cp_outside && s_cand[1](1) > end_cp_inside) || (s_cand[1](1) < end_cp_outside && s_cand[1](1) > end_cp_inside)) s.push_back(s_cand[1]); + if ((s_cand[3](1) < end_cp_outside && s_cand[3](1) > end_cp_inside) || (s_cand[3](1) < end_cp_outside && s_cand[3](1) > end_cp_inside)) s.push_back(s_cand[3]); + // project to line segment + if (s.size() == 2) { + for (size_t i = 0; i < 2; i++) { + if (p(i) > std::max(s[0](i), s[1](i))) p(i) = std::max(s[0](i), s[1](i)); + else if (p(i) < std::min(s[0](i), s[1](i))) p(i) = std::min(s[0](i), s[1](i)); + } + } else { + std::cerr << "Cannot calculate projected point" << std::endl; + } + } + } + // preprev foot frame + p = (prev_fs_pos + prev_fs_rot * hrp::Vector3(p(0), p(1), 0)).head(2); + } + // project to line segment + for (size_t i = 0; i < 2; i++) { + if (p(i) > std::max(ch[v_a](i), ch[v_b](i))) p(i) = std::max(ch[v_a](i), ch[v_b](i)); + else if (p(i) < std::min(ch[v_a](i), ch[v_b](i))) p(i) = std::min(ch[v_a](i), ch[v_b](i)); + } + // prev foot frame + hrp::Vector3 end_cp = prev_fs_rot.transpose() * (hrp::Vector3(p(0), p(1), 0) - prev_fs_pos); + double end_cp_front = std::exp(omega * t) * cp(0) - (std::exp(omega * t) - 1) * safe_leg_margin[0]; + double end_cp_back = std::exp(omega * t) * cp(0) + (std::exp(omega * t) - 1) * safe_leg_margin[1]; + double end_cp_outside = std::exp(omega * t) * cp(1) - (cur_sup == RLEG ? -1 : 1) * (std::exp(omega * t) - 1) * safe_leg_margin[2]; + double end_cp_inside = std::exp(omega * t) * cp(1) - (cur_sup == RLEG ? 1 : -1) * (std::exp(omega * t) - 1) * safe_leg_margin[3]; + bool is_change_time = false; + double xz_max, l_max; + // calc remain_time + if (end_cp(0) < end_cp_front) { + xz_max = safe_leg_margin[0]; + l_max = end_cp(0); + double tmp_t = std::log((l_max - xz_max) / (cp(0) - xz_max)) / omega; + if (std::fabs(tmp_t - t) > tmp_dt) tmp_dt = tmp_t - t; + is_change_time = true; + } else if (end_cp(0) > end_cp_back) { + xz_max = -1 * safe_leg_margin[1]; + l_max = end_cp(0); + double tmp_t = std::log((l_max - xz_max) / (cp(0) - xz_max)) / omega; + if (std::fabs(tmp_t - t) > tmp_dt) tmp_dt = tmp_t - t; + is_change_time = true; + } + if ((cur_sup == RLEG ? -1 : 1) * end_cp(1) < (cur_sup == RLEG ? -1 : 1) * end_cp_outside) { + xz_max = (cur_sup == RLEG ? -1 : 1) * safe_leg_margin[2]; + l_max = end_cp(1); + double tmp_t = std::log((l_max - xz_max) / (cp(1) - xz_max)) / omega; + if (std::fabs(tmp_t - t) > tmp_dt) tmp_dt = tmp_t - t; + is_change_time = true; + } else if ((cur_sup == RLEG ? -1 : 1) * end_cp(1) > (cur_sup == RLEG ? -1 : 1) * end_cp_inside) { + xz_max = (cur_sup == RLEG ? 1 : -1) * safe_leg_margin[3]; + l_max = end_cp(1); + double tmp_t = std::log((l_max - xz_max) / (cp(1) - xz_max)) / omega; + if (std::fabs(tmp_t - t) > tmp_dt) tmp_dt = tmp_t - t; + is_change_time = true; + } + if (is_change_time) { + if (tmp_dt < 0) min_time_check(tmp_dt); + t = t + tmp_dt; + + // calc target pos according t + hrp::Vector3 target_p = end_cp; + end_cp_front = std::exp(omega * t) * cp(0) - (std::exp(omega * t) - 1) * safe_leg_margin[0]; + end_cp_back = std::exp(omega * t) * cp(0) + (std::exp(omega * t) - 1) * safe_leg_margin[1]; + end_cp_outside = std::exp(omega * t) * cp(1) - (cur_sup == RLEG ? -1 : 1) * (std::exp(omega * t) - 1) * safe_leg_margin[2]; + end_cp_inside = std::exp(omega * t) * cp(1) - (cur_sup == RLEG ? 1 : -1) * (std::exp(omega * t) - 1) * safe_leg_margin[3]; + if (end_cp(0) < end_cp_front) { + xz_max = safe_leg_margin[0]; + target_p(0) = std::exp(omega * t) * cp(0) + (1 - std::exp(omega * t)) * xz_max; + } else if (end_cp(0) > end_cp_back) { + xz_max = -1 * safe_leg_margin[1]; + target_p(0) = std::exp(omega * t) * cp(0) + (1 - std::exp(omega * t)) * xz_max; + } + if ((cur_sup == RLEG ? -1 : 1) * end_cp(1) < (cur_sup == RLEG ? -1 : 1) * end_cp_outside) { + xz_max = (cur_sup == RLEG ? -1 : 1) * safe_leg_margin[2]; + target_p(1) = std::exp(omega * t) * cp(1) + (1 - std::exp(omega * t)) * xz_max; + } else if ((cur_sup == RLEG ? -1 : 1) * end_cp(1) > (cur_sup == RLEG ? -1 : 1) * end_cp_inside) { + xz_max = (cur_sup == RLEG ? 1 : -1) * safe_leg_margin[3]; + target_p(1) = std::exp(omega * t) * cp(1) + (1 - std::exp(omega * t)) * xz_max; + } + // preprev foot frame + target_p = prev_fs_pos + prev_fs_rot * target_p; + short_of_footstep.head(2) = p - target_p.head(2); + } + } + p += offset.head(2); + return false; + } + }; + std::vector calc_intersect_convex (std::vector& P, std::vector& Q); + bool is_intersect (Eigen::Vector2d& r, const Eigen::Vector2d& a0, const Eigen::Vector2d& a1, const Eigen::Vector2d& b0, const Eigen::Vector2d& b1); + // Compare Vector2d for sorting lexicographically + static bool compare_eigen2d(const Eigen::Vector2d& lv, const Eigen::Vector2d& rv) + { + return lv(0) < rv(0) || (lv(0) == rv(0) && lv(1) < rv(1)); + }; + // Calculate 2D convex hull based on Andrew's algorithm + // Assume that the order of vs, ee, and cs is the same + void calc_convex_hull (const std::vector >& vs, const std::vector& cs, const std::vector& ee_pos, const std::vector & ee_rot) + { + // transform coordinate + std::vector tvs; + hrp::Vector3 tpos; + tvs.reserve(cs.size()*vs[0].size()); + for (size_t i = 0; i < cs.size(); i++) { + if (cs[i]) { + for (size_t j = 0; j < vs[i].size(); j++) { + tpos = ee_pos[i] + ee_rot[i] * hrp::Vector3(vs[i][j](0), vs[i][j](1), 0.0); + tvs.push_back(tpos.head(2)); + } + } + } + // calculate convex hull + int n_tvs = tvs.size(), n_ch = 0; + convex_hull.resize(2*n_tvs); + std::sort(tvs.begin(), tvs.end(), compare_eigen2d); + for (int i = 0; i < n_tvs; convex_hull[n_ch++] = tvs[i++]) + while (n_ch >= 2 && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--; + for (int i = n_tvs-2, j = n_ch+1; i >= 0; convex_hull[n_ch++] = tvs[i--]) + while (n_ch >= j && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--; + convex_hull.resize(n_ch-1); + }; + // Calculate closest boundary point on the convex hull + bool calc_closest_boundary_point (Eigen::Vector2d& p, const std::vector& ch, size_t& right_idx, size_t& left_idx) { + size_t n_ch = ch.size(); + Eigen::Vector2d cur_closest_point; + for (size_t i = 0; i < n_ch; i++) { + switch(calcProjectedPoint(cur_closest_point, p, ch[left_idx], ch[right_idx])) { + case MIDDLE: + p = cur_closest_point; + return true; + case LEFT: + right_idx = left_idx; + left_idx = (left_idx + 1) % n_ch; + if ((p - ch[right_idx]).dot(ch[left_idx] - ch[right_idx]) <= 0) { + p = cur_closest_point; + return true; + } + case RIGHT: + left_idx = right_idx; + right_idx = (right_idx - 1) % n_ch; + if ((p - ch[left_idx]).dot(ch[right_idx] - ch[left_idx]) <= 0) { + p = cur_closest_point; + return true; + } + } + } + return false; + }; + // Retrieving only + hrp::Vector3 calcDampingControl (const hrp::Vector3 prev_d, const double TT) + { + return - 1/TT * prev_d * dt + prev_d; + }; /* parameter setting */ void set_default_step_time (const double _default_step_time) { default_step_time = _default_step_time; }; void set_default_double_support_ratio_before (const double _default_double_support_ratio_before) { default_double_support_ratio_before = _default_double_support_ratio_before; }; @@ -1022,7 +1723,8 @@ namespace rats void set_default_zmp_offsets(const std::vector& tmp) { rg.set_default_zmp_offsets(tmp); }; void set_toe_zmp_offset_x (const double _off) { rg.set_toe_zmp_offset_x(_off); }; void set_heel_zmp_offset_x (const double _off) { rg.set_heel_zmp_offset_x(_off); }; - void set_use_toe_heel_transition (const double _u) { rg.set_use_toe_heel_transition(_u); }; + void set_use_toe_heel_transition (const bool _u) { rg.set_use_toe_heel_transition(_u); }; + void set_use_toe_heel_auto_set (const bool _u) { rg.set_use_toe_heel_auto_set(_u); lcg.set_use_toe_heel_auto_set(_u); }; void set_zmp_weight_map (const std::map _map) { rg.set_zmp_weight_map(_map); }; void set_default_step_height(const double _tmp) { lcg.set_default_step_height(_tmp); }; void set_default_top_ratio(const double _tmp) { lcg.set_default_top_ratio(_tmp); }; @@ -1034,25 +1736,37 @@ namespace rats { offset_vel_param.set(vel_x, vel_y, vel_theta); }; - void set_stride_parameters (const double _stride_fwd_x, const double _stride_y, const double _stride_theta, const double _stride_bwd_x) + void set_stride_parameters (const double _stride_fwd_x, const double _stride_outside_y, const double _stride_outside_theta, + const double _stride_bwd_x, const double _stride_inside_y, const double _stride_inside_theta) { footstep_param.stride_fwd_x = _stride_fwd_x; - footstep_param.stride_y = _stride_y; - footstep_param.stride_theta = _stride_theta; + footstep_param.stride_outside_y = _stride_outside_y; + footstep_param.stride_outside_theta = _stride_outside_theta; footstep_param.stride_bwd_x = _stride_bwd_x; + footstep_param.stride_inside_y = _stride_inside_y; + footstep_param.stride_inside_theta = _stride_inside_theta; }; void set_use_inside_step_limitation(const bool uu) { use_inside_step_limitation = uu; }; void set_default_orbit_type (const orbit_type type) { lcg.set_default_orbit_type(type); }; void set_swing_trajectory_delay_time_offset (const double _time_offset) { lcg.set_swing_trajectory_delay_time_offset(_time_offset); }; void set_swing_trajectory_final_distance_weight (const double _final_distance_weight) { lcg.set_swing_trajectory_final_distance_weight(_final_distance_weight); }; + void set_swing_trajectory_time_offset_xy2z (const double _tmp) { lcg.set_swing_trajectory_time_offset_xy2z(_tmp); }; void set_stair_trajectory_way_point_offset (const hrp::Vector3 _offset) { lcg.set_stair_trajectory_way_point_offset(_offset); }; + void set_rectangle_trajectory_way_point_offset (const hrp::Vector3 _offset) { lcg.set_rectangle_trajectory_way_point_offset(_offset); }; + void set_rectangle_goal_off (const hrp::Vector3 _offset) { lcg.set_rectangle_goal_off(_offset); }; + void set_rectangle_time_smooth_offset(const double _offset) { lcg.set_rectangle_time_smooth_offset(_offset); }; void set_cycloid_delay_kick_point_offset (const hrp::Vector3 _offset) { lcg.set_cycloid_delay_kick_point_offset(_offset); }; void set_gravitational_acceleration (const double ga) { gravitational_acceleration = ga; }; void set_toe_pos_offset_x (const double _offx) { lcg.set_toe_pos_offset_x(_offx); }; void set_heel_pos_offset_x (const double _offx) { lcg.set_heel_pos_offset_x(_offx); }; void set_toe_angle (const double _angle) { lcg.set_toe_angle(_angle); }; void set_heel_angle (const double _angle) { lcg.set_heel_angle(_angle); }; - bool set_toe_heel_phase_ratio (const std::vector& ratio) { return thp.set_toe_heel_phase_ratio(ratio); }; + bool set_toe_heel_phase_ratio (const std::vector& ratio) + { + bool lcgret = lcg.set_toe_heel_phase_ratio(ratio); + bool rgret = rg.set_toe_heel_phase_ratio(ratio); + return lcgret && rgret; + }; void set_use_toe_joint (const bool ut) { lcg.set_use_toe_joint(ut); }; void set_leg_default_translate_pos (const std::vector& off) { footstep_param.leg_default_translate_pos = off;}; void set_optional_go_pos_finalize_footstep_num (const size_t num) { optional_go_pos_finalize_footstep_num = num; }; @@ -1072,6 +1786,110 @@ namespace rats append_finalize_footstep(overwrite_footstep_nodes_list); print_footstep_nodes_list(overwrite_footstep_nodes_list); }; + void set_leg_margin (const double _leg_margin[4]) { + for (size_t i = 0; i < 4; i++) { + leg_margin[i] = _leg_margin[i]; + } + }; + void set_safe_leg_margin (const double _leg_margin[4]) { + for (size_t i = 0; i < 4; i++) { + safe_leg_margin[i] = _leg_margin[i]; + } + }; + void set_stride_limitation_for_circle_type (const double (&_stride_limitation_for_circle_type)[5]) { + for (size_t i = 0; i < 5; i++) { + stride_limitation_for_circle_type[i] = _stride_limitation_for_circle_type[i]; + } + }; + void set_overwritable_stride_limitation (const double (&_overwritable_stride_limitation)[5]) { + for (size_t i = 0; i < 5; i++) { + overwritable_stride_limitation[i] = _overwritable_stride_limitation[i]; + } + }; + void set_footstep_modification_gain (const double _footstep_modification_gain) { footstep_modification_gain = _footstep_modification_gain; }; + void set_cp_check_margin (const double (&_cp_check_margin)[2]) { + for (size_t i=0; i < 2; i++) { + cp_check_margin[i] = _cp_check_margin[i]; + } + }; + void set_act_contact_states (const std::vector& _act_contact_states) { + if (act_contact_states.empty()) act_contact_states.resize(_act_contact_states.size()); + for (size_t i = 0; i < act_contact_states.size(); i++) { + act_contact_states[i] = _act_contact_states[i]; + } + lcg.set_act_contact_states(_act_contact_states); + }; + void set_use_stride_limitation (const bool _use_stride_limitation) { use_stride_limitation = _use_stride_limitation; }; + void set_modify_footsteps (const bool _modify_footsteps) { modify_footsteps = _modify_footsteps; }; + void set_min_time_mgn (const double _t) { min_time_mgn = _t; }; + void set_min_time (const double _t) { min_time = _t; }; + void set_margin_time_ratio (const double _margin_time_ratio) { margin_time_ratio = _margin_time_ratio; }; + void set_diff_cp (const hrp::Vector3& _cp) { diff_cp = _cp; }; + void set_stride_limitation_type (const stride_limitation_type _tmp) { default_stride_limitation_type = _tmp; }; + void set_toe_check_thre (const double _a) { thtc.set_toe_check_thre(_a); }; + void set_heel_check_thre (const double _a) { thtc.set_heel_check_thre(_a); }; + void set_act_vel_ratio (const double ratio) { act_vel_ratio = ratio; }; + void set_vertices (const std::vector >& vs) { foot_vertices = vs; }; + void get_vertices (std::vector >& vs) { vs = foot_vertices; }; + void set_total_mass (const double _m) { total_mass = _m; }; + void set_use_disturbance_compensation (const bool _use) { use_disturbance_compensation = _use; }; + void set_dc_gain (const double _gain) { dc_gain = _gain; }; + void set_dcm_offset (const double _off) { dcm_offset = _off; }; + void set_vel_foot_offset (const hrp::Vector3& _off, const leg_type _l_r) { footstep_param.vel_foot_offset[_l_r] = _off; }; + void set_is_vision_updated (const bool _t) { is_vision_updated = _t; }; + void set_rel_landing_height (const hrp::Vector3& _pos) { rel_landing_height = _pos; }; + void set_rel_landing_normal (const hrp::Vector3& _n) { rel_landing_normal = _n; }; + void set_steppable_region (const OpenHRP::TimedSteppableRegion& _region) { + leg_type next_sup = (_region.data.l_r == 0 ? LLEG : RLEG); + steppable_region[next_sup].resize(_region.data.region.length()); + steppable_height[next_sup].resize(_region.data.region.length(), 0.0); + for (size_t i = 0; i < steppable_region[next_sup].size(); i++) { + double height_sum = 0.0; + steppable_region[next_sup][i].resize(_region.data.region[i].length()/3);; + for (size_t j = 0; j < steppable_region[next_sup][i].size(); j++) { + steppable_region[next_sup][i][j](0) = _region.data.region[i][3*j]; + steppable_region[next_sup][i][j](1) = _region.data.region[i][3*j+1]; + height_sum += _region.data.region[i][3*j+2]; + } + if (steppable_region[next_sup][i].size() > 0) steppable_height[next_sup][i] = height_sum / steppable_region[next_sup][i].size(); // average height + } + lr_region[next_sup] = true; + }; + void set_vertices_from_leg_margin () + { + std::vector > vec; + // RLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_margin[0], leg_margin[3])); + tvec.push_back(Eigen::Vector2d(leg_margin[0], -1*leg_margin[2])); + tvec.push_back(Eigen::Vector2d(-1*leg_margin[1], -1*leg_margin[2])); + tvec.push_back(Eigen::Vector2d(-1*leg_margin[1], leg_margin[3])); + vec.push_back(tvec); + } + // LLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_margin[0], leg_margin[2])); + tvec.push_back(Eigen::Vector2d(leg_margin[0], -1*leg_margin[3])); + tvec.push_back(Eigen::Vector2d(-1*leg_margin[1], -1*leg_margin[3])); + tvec.push_back(Eigen::Vector2d(-1*leg_margin[1], leg_margin[2])); + vec.push_back(tvec); + } + set_vertices(vec); + }; + void set_is_emergency_step (const bool _is_emergency_step) { is_emergency_step = _is_emergency_step; }; + void set_emergency_step_time (const double _emergency_step_time[3]) { + for (size_t i = 0; i < 3; i++) { + emergency_step_time[i] = _emergency_step_time[i]; + } + } + void set_sum_d_footstep_thre (const hrp::Vector3& _thre) { sum_d_footstep_thre = _thre; }; + void set_footstep_check_delta (const hrp::Vector3& _delta) { footstep_check_delta = _delta; }; + void set_use_act_states() { lcg.set_use_act_states(use_act_states); }; + void set_is_stop_early_foot(const bool _is_stop_early_foot) { lcg.set_is_stop_early_foot(_is_stop_early_foot); }; + void set_use_force_sensor(const bool _use_force_sensor) { lcg.set_use_force_sensor(_use_force_sensor); }; + void set_is_early_touch(const bool _is_early_touch, const leg_type _lr) { lcg.set_is_early_touch(_is_early_touch, _lr); }; /* Get overwritable footstep index. For example, if overwritable_footstep_index_offset = 1, overwrite next footstep. If overwritable_footstep_index_offset = 0, overwrite current swinging footstep. */ size_t get_overwritable_index () const { @@ -1115,10 +1933,14 @@ namespace rats return hrp::Vector3(refcog_vel[0], refcog_vel[1], refcog_vel[2]); }; hrp::Vector3 get_cog_acc () const { - double refcog_acc[3]; - preview_controller_ptr->get_refcog_acc(refcog_acc); - return hrp::Vector3(refcog_acc[0], refcog_acc[1], refcog_acc[2]); + hrp::Vector3 refcog_acc; + // preview_controller_ptr->get_refcog_acc(refcog_acc); + foot_guided_controller_ptr->get_acc(refcog_acc); + return refcog_acc; }; + bool is_jumping_phase () const { return (jump_phase == JUMPING); }; + bool is_steppable_while_jumping () const { return (!is_jumping || + (is_jumping && jump_phase == AFTER_JUMP && jump_remain_time < 0.8*jump_recover_time && act_contact_states[0] && act_contact_states[1])); }; const hrp::Vector3& get_refzmp () const { return refzmp;}; hrp::Vector3 get_cart_zmp () const { @@ -1141,9 +1963,31 @@ namespace rats } return ret; }; + // Get foot zmp offsets by checking whether given EE name is swing or support + bool get_swing_support_foot_zmp_offsets_from_ee_name (hrp::Vector3& ret, const std::string& ee_name) const + { + leg_type lt = get_leg_type_from_ee_name(ee_name); + std::vector::const_iterator it = std::find(lcg.get_support_leg_types().begin(), lcg.get_support_leg_types().end(), lt); + if (it != lcg.get_support_leg_types().end()) { // If support leg + ret = get_support_foot_zmp_offsets()[std::distance(lcg.get_support_leg_types().begin(), it)]; + } else { + it = std::find(lcg.get_swing_leg_types().begin(), lcg.get_swing_leg_types().end(), lt); + if (it != lcg.get_swing_leg_types().end()) { // If swing leg + ret = get_swing_foot_zmp_offsets()[std::distance(lcg.get_swing_leg_types().begin(), it)]; + } else { // Otherwise + return false; + } + } + return true; + }; double get_toe_zmp_offset_x () const { return rg.get_toe_zmp_offset_x(); }; double get_heel_zmp_offset_x () const { return rg.get_heel_zmp_offset_x(); }; bool get_use_toe_heel_transition () const { return rg.get_use_toe_heel_transition(); }; + bool get_use_toe_heel_auto_set () const { return rg.get_use_toe_heel_auto_set(); }; + size_t get_remain_count () const { return remain_count; }; + size_t get_touchoff_remain_count (size_t idx) const { return touchoff_remain_count[idx]; }; + bool is_before_step_phase () const { return lcg.get_lcg_count() > static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1; }; + leg_type get_cur_leg () const { return footstep_nodes_list[lcg.get_footstep_index()].front().l_r; }; const std::map get_zmp_weight_map () const { return rg.get_zmp_weight_map(); }; void proc_zmp_weight_map_interpolation () { return rg.proc_zmp_weight_map_interpolation(); }; std::vector get_footstep_front_leg_names () const { @@ -1169,20 +2013,34 @@ namespace rats const coordinates get_dst_foot_midcoords() const /* get foot_midcoords calculated from swing_leg_dst_coords */ { coordinates tmp(lcg.get_swing_leg_dst_steps().front().worldcoords); - tmp.pos += tmp.rot * hrp::Vector3(-1*footstep_param.leg_default_translate_pos[lcg.get_swing_leg_dst_steps().front().l_r]); + // tmp.pos += tmp.rot * hrp::Vector3(-1*footstep_param.leg_default_translate_pos[lcg.get_swing_leg_dst_steps().front().l_r]); + tmp.pos += tmp.rot * hrp::Vector3(-1*footstep_param.vel_foot_offset[lcg.get_swing_leg_dst_steps().front().l_r]); return tmp; }; - void get_swing_support_mid_coords(coordinates& ret) const { lcg.get_swing_support_mid_coords(ret); }; - void get_stride_parameters (double& _stride_fwd_x, double& _stride_y, double& _stride_theta, double& _stride_bwd_x) const + void get_swing_support_mid_coords(coordinates& ret) const { + lcg.get_swing_support_mid_coords(ret); + if (is_wheeling) ret.pos += d_wheel_pos; + }; + void get_jump_mid_coords(coordinates& ret) const { ret = jump_midcoords; }; + void get_wheel_mid_coords(coordinates& ret) const { ret = wheel_midcoords; }; + void get_stride_parameters (double& _stride_fwd_x, double& _stride_outside_y, double& _stride_outside_theta, + double& _stride_bwd_x, double& _stride_inside_y, double& _stride_inside_theta) const { _stride_fwd_x = footstep_param.stride_fwd_x; - _stride_y = footstep_param.stride_y; - _stride_theta = footstep_param.stride_theta; + _stride_outside_y = footstep_param.stride_outside_y; + _stride_outside_theta = footstep_param.stride_outside_theta; _stride_bwd_x = footstep_param.stride_bwd_x; + _stride_inside_y = footstep_param.stride_inside_y; + _stride_inside_theta = footstep_param.stride_inside_theta; }; size_t get_footstep_index() const { return lcg.get_footstep_index(); }; size_t get_lcg_count() const { return lcg.get_lcg_count(); }; double get_current_swing_time(const size_t idx) const { return lcg.get_current_swing_time(idx); }; + // Get current swing time by checking whether given EE name is swing or support + double get_current_swing_time_from_ee_name (const std::string ee_name) const + { + return get_current_swing_time( get_leg_type_from_ee_name(ee_name) ); + }; std::vector get_current_support_states() const { return lcg.get_current_support_states();}; double get_default_step_time () const { return default_step_time; }; double get_default_step_height () const { return lcg.get_default_step_height(); }; @@ -1192,6 +2050,9 @@ namespace rats double get_default_double_support_static_ratio_after () const { return default_double_support_static_ratio_after; }; double get_default_double_support_ratio_swing_before () const {return default_double_support_ratio_swing_before; }; double get_default_double_support_ratio_swing_after () const {return default_double_support_ratio_swing_after; }; + hrp::Vector3 get_flywheel_tau () const { return flywheel_tau; }; + bool get_use_roll_flywheel () const { return use_roll_flywheel; }; + bool get_use_pitch_flywheel () const { return use_pitch_flywheel; }; std::vector< std::vector > get_remaining_footstep_nodes_list () const { std::vector< std::vector > fsnl; @@ -1205,7 +2066,11 @@ namespace rats orbit_type get_default_orbit_type () const { return lcg.get_default_orbit_type(); }; double get_swing_trajectory_delay_time_offset () const { return lcg.get_swing_trajectory_delay_time_offset(); }; double get_swing_trajectory_final_distance_weight () const { return lcg.get_swing_trajectory_final_distance_weight(); }; + double get_swing_trajectory_time_offset_xy2z () const { return lcg.get_swing_trajectory_time_offset_xy2z(); }; hrp::Vector3 get_stair_trajectory_way_point_offset () const { return lcg.get_stair_trajectory_way_point_offset(); }; + hrp::Vector3 get_rectangle_trajectory_way_point_offset () const { return lcg.get_rectangle_trajectory_way_point_offset(); }; + hrp::Vector3 get_rectangle_goal_off () const { return lcg.get_rectangle_goal_off(); }; + double get_rectangle_time_smooth_offset () const { return lcg.get_rectangle_time_smooth_offset(); }; hrp::Vector3 get_cycloid_delay_kick_point_offset () const { return lcg.get_cycloid_delay_kick_point_offset(); }; double get_gravitational_acceleration () const { return gravitational_acceleration; } ; double get_toe_pos_offset_x () const { return lcg.get_toe_pos_offset_x(); }; @@ -1213,21 +2078,156 @@ namespace rats double get_toe_angle () const { return lcg.get_toe_angle(); }; double get_heel_angle () const { return lcg.get_heel_angle(); }; double get_foot_dif_rot_angle () const { return lcg.get_foot_dif_rot_angle(); }; - void get_toe_heel_phase_ratio (std::vector& ratio) const { thp.get_toe_heel_phase_ratio(ratio); }; - int get_NUM_TH_PHASES () const { return thp.get_NUM_TH_PHASES(); }; + void get_toe_heel_phase_ratio (std::vector& ratio) const { lcg.get_toe_heel_phase_ratio(ratio); }; + int get_NUM_TH_PHASES () const { return NUM_TH_PHASES; }; bool get_use_toe_joint () const { return lcg.get_use_toe_joint(); }; + double get_current_toe_heel_ratio () const { return lcg.get_current_toe_heel_ratio(get_use_toe_heel_transition()); }; + // Get current toe heel ratio by checking whether given EE name is swing or support + bool get_current_toe_heel_ratio_from_ee_name (double& ret, const std::string& ee_name) const + { + leg_type lt = get_leg_type_from_ee_name(ee_name); + if (std::find(lcg.get_support_leg_types().begin(), lcg.get_support_leg_types().end(), lt)!=lcg.get_support_leg_types().end()) { // If support + ret = rats::no_using_toe_heel_ratio; + } else if (std::find(lcg.get_swing_leg_types().begin(), lcg.get_support_leg_types().end(), lt)!=lcg.get_swing_leg_types().end()) { // If swing + ret = get_current_toe_heel_ratio(); + } else { // Otherwise + return false; + } + return true; + }; void get_leg_default_translate_pos (std::vector& off) const { off = footstep_param.leg_default_translate_pos; }; size_t get_overwritable_footstep_index_offset () const { return overwritable_footstep_index_offset; }; const std::vector calc_counter_leg_types_from_footstep_nodes (const std::vector& fns, std::vector _all_limbs) const; const std::map get_leg_type_map () const { return leg_type_map; }; size_t get_optional_go_pos_finalize_footstep_num () const { return optional_go_pos_finalize_footstep_num; }; bool is_finalizing (const double tm) const { return ((preview_controller_ptr->get_delay()*2 - default_step_time/dt)-finalize_count) <= (tm/dt)-1; }; - size_t get_overwrite_check_timing () const { return static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 0.5) - 1;}; // Almost middle of step time + size_t get_overwrite_check_timing () const { return static_cast(footstep_nodes_list[lcg.get_footstep_index()][0].step_time/dt * 1.0) - 1;}; // Almost fist of step time + double get_leg_margin (const size_t idx) const { return leg_margin[idx]; }; + double get_safe_leg_margin (const size_t idx) const { return safe_leg_margin[idx]; }; + double get_stride_limitation_for_circle_type (const size_t idx) const { return stride_limitation_for_circle_type[idx]; }; + double get_overwritable_stride_limitation (const size_t idx) const { return overwritable_stride_limitation[idx]; }; + double get_footstep_modification_gain () const { return footstep_modification_gain; }; + double get_cp_check_margin (const size_t idx) const { return cp_check_margin[idx]; }; + bool get_modify_footsteps () const { return modify_footsteps; }; + double get_min_time_mgn () const { return min_time_mgn; }; + double get_min_time () const { return min_time; }; + double get_margin_time_ratio () const { return margin_time_ratio; }; + bool get_use_stride_limitation () const { return use_stride_limitation; }; + stride_limitation_type get_stride_limitation_type () const { return default_stride_limitation_type; }; + double get_toe_check_thre () const { return thtc.get_toe_check_thre(); }; + double get_heel_check_thre () const { return thtc.get_heel_check_thre(); }; + size_t get_falling_direction () const {return falling_direction;}; + size_t get_step_num () const {return footstep_nodes_list.size(); }; + bool get_is_single_walking () const { return (lcg.get_footstep_index() > 0 && lcg.get_footstep_index() < footstep_nodes_list.size()-2); }; + bool get_is_more_than_1st_step () const { return (lcg.get_footstep_index() > 1); }; + // Get ee coords by checking whether given EE name is swing or support + bool get_swing_support_ee_coords_from_ee_name (hrp::Vector3& cpos, hrp::Matrix33& crot, const std::string& ee_name) const + { + leg_type lt = get_leg_type_from_ee_name(ee_name); + if (std::find(lcg.get_support_leg_types().begin(), lcg.get_support_leg_types().end(), lt) != lcg.get_support_leg_types().end()) { // If support + coordinates tmpc = std::find_if(lcg.get_support_leg_steps().begin(), lcg.get_support_leg_steps().end(), (&boost::lambda::_1->* &step_node::l_r == lt))->worldcoords; + cpos = tmpc.pos; + crot = tmpc.rot; + } else if (std::find(lcg.get_swing_leg_types().begin(), lcg.get_swing_leg_types().end(), lt) != lcg.get_swing_leg_types().end()) { // If swing + coordinates tmpc = std::find_if(lcg.get_swing_leg_steps().begin(), lcg.get_swing_leg_steps().end(), (&boost::lambda::_1->* &step_node::l_r == lt))->worldcoords; + cpos = tmpc.pos; + crot = tmpc.rot; + } else { // Otherwise + return false; + } + if (is_wheeling) { + cpos += d_wheel_pos; + } + return true; + }; + bool get_jump_ee_coords_from_ee_name (hrp::Vector3& cpos, hrp::Matrix33& crot, const std::string& ee_name) const + { + if (ee_name == "lleg") { // If support + cpos = initial_support_leg.worldcoords.pos + d_jump_foot_pos; + crot = initial_support_leg.worldcoords.rot; + } else if (ee_name == "rleg") { // If swing + cpos = initial_swing_leg.worldcoords.pos + d_jump_foot_pos; + crot = initial_swing_leg.worldcoords.rot; + } else { // Otherwise + return false; + } + return true; + }; + bool get_wheel_ee_coords_from_ee_name (hrp::Vector3& cpos, hrp::Matrix33& crot, const std::string& ee_name) const + { + if (ee_name == "lleg") { // If support + cpos = initial_support_leg.worldcoords.pos + d_wheel_pos; + crot = initial_support_leg.worldcoords.rot; + } else if (ee_name == "rleg") { // If swing + cpos = initial_swing_leg.worldcoords.pos + d_wheel_pos; + crot = initial_swing_leg.worldcoords.rot; + } else { // Otherwise + return false; + } + return true; + }; + // Get current support state (true=support, false=swing) by checking whether given EE name is swing or support + bool get_current_support_state_from_ee_name (const std::string& ee_name) const + { + leg_type lt = get_leg_type_from_ee_name(ee_name); + std::vector tmp = lcg.get_current_support_states(); + return std::find(tmp.begin(), tmp.end(), lt) != tmp.end(); + }; + double get_act_vel_ratio () { return act_vel_ratio; }; + bool get_use_disturbance_compensation () { return use_disturbance_compensation; }; + double get_dc_gain () { return dc_gain; }; + double get_dcm_offset () { return dcm_offset; }; + double get_tmp (const size_t idx) {return tmp[idx];} + double get_current_landing_pos (const size_t idx) { + return footstep_nodes_list[lcg.get_footstep_index()].front().worldcoords.pos(idx); + } + double get_emergency_step_time (const size_t idx) const { return emergency_step_time[idx]; }; + void get_sum_d_footstep_thre (hrp::Vector3& thre) { thre = sum_d_footstep_thre; }; + void get_footstep_check_delta (hrp::Vector3& delta) { delta = footstep_check_delta; }; + void get_rel_landing_pos (hrp::Vector3& pos, int& l_r) { + pos = rel_landing_pos; + l_r = cur_supporting_foot; + }; + void get_end_cog_state (hrp::Vector3& pos, hrp::Vector3& vel, int& l_r) { + pos = end_cog; + vel = end_cogvel; + l_r = cur_supporting_foot; + }; + void get_current_steppable_region (OpenHRP::TimedSteppableRegion& _region) { + leg_type cur_sup = footstep_nodes_list[lcg.get_footstep_index()-1].front().l_r; + if (lcg.get_footstep_index() > 0 && lr_region[cur_sup]) { + _region.data.l_r = (cur_sup == RLEG ? 0 : 1); + hrp::Vector3 preprev_fs_pos = (lcg.get_footstep_index()==1 ? lcg.get_swing_leg_src_steps().front() : footstep_nodes_list[lcg.get_footstep_index()-2].front()).worldcoords.pos; + _region.data.region.length(steppable_region[cur_sup].size()); + for (size_t i = 0; i < steppable_region[cur_sup].size(); i++) { + _region.data.region[i].length(steppable_region[cur_sup][i].size()*3); + for (size_t j = 0; j < steppable_region[cur_sup][i].size(); j++) { + _region.data.region[i][3*j] = steppable_region[cur_sup][i][j](0) + preprev_fs_pos(0); + _region.data.region[i][3*j+1] = steppable_region[cur_sup][i][j](1) + preprev_fs_pos(1); + _region.data.region[i][3*j+2] = steppable_height[cur_sup][i] + preprev_fs_pos(2); + } + } + } + }; + double get_cur_wheel_pos_x () { return cur_wheel_pos_x; }; + hrp::Vector3 get_d_wheel_pos () { return d_wheel_pos; }; + + +#ifdef FOR_TESTGAITGENERATOR + size_t get_one_step_count() const { return lcg.get_one_step_count(); }; + void get_footstep_nodes_list (std::vector< std::vector > & fsl) const + { + fsl = footstep_nodes_list; + }; + double get_toe_heel_dif_angle () const { return lcg.get_toe_heel_dif_angle(); }; + std::vector get_default_zmp_offsets() const { return rg.get_default_zmp_offsets(); }; +#endif // FOR_TESTGAITGENERATOR void print_param (const std::string& print_str = "") const { - double stride_fwd_x, stride_y, stride_th, stride_bwd_x; - get_stride_parameters(stride_fwd_x, stride_y, stride_th, stride_bwd_x); - std::cerr << "[" << print_str << "] stride_parameter = " << stride_fwd_x << "[m], " << stride_y << "[m], " << stride_th << "[deg], " << stride_bwd_x << "[m]" << std::endl; + double stride_fwd_x, stride_outside_y, stride_outside_th, stride_bwd_x, stride_inside_y, stride_inside_th; + get_stride_parameters(stride_fwd_x, stride_outside_y, stride_outside_th, stride_bwd_x, stride_inside_y, stride_inside_th); + std::cerr << "[" << print_str << "] stride_parameter = " << stride_fwd_x << "[m], " << stride_outside_y << "[m], " << stride_outside_th << "[deg], " + << stride_bwd_x << "[m], " << stride_inside_y << "[m], " << stride_inside_th << "[deg]" << std::endl; std::cerr << "[" << print_str << "] leg_default_translate_pos = "; for (size_t i = 0; i < footstep_param.leg_default_translate_pos.size(); i++) { std::cerr << footstep_param.leg_default_translate_pos[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")); @@ -1252,7 +2252,8 @@ namespace rats } else if (get_default_orbit_type() == CROSS) { std::cerr << "CROSS" << std::endl; } - std::cerr << "[" << print_str << "] swing_trajectory_delay_time_offset = " << get_swing_trajectory_delay_time_offset() << "[s], swing_trajectory_final_distance_weight = " << get_swing_trajectory_final_distance_weight() << std::endl; + std::cerr << "[" << print_str << "] swing_trajectory_delay_time_offset = " << get_swing_trajectory_delay_time_offset() << "[s], swing_trajectory_final_distance_weight = " << get_swing_trajectory_final_distance_weight() + << ", swing_trajectory_time_offset_xy2z = " << get_swing_trajectory_time_offset_xy2z() << std::endl; hrp::Vector3 tmpv; tmpv = get_stair_trajectory_way_point_offset(); std::cerr << "[" << print_str << "] stair_trajectory_way_point_offset = " << tmpv.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; @@ -1263,13 +2264,20 @@ namespace rats std::cerr << "[" << print_str << "] toe_zmp_offset_x = " << get_toe_zmp_offset_x() << "[mm], heel_zmp_offset_x = " << get_heel_zmp_offset_x() << "[mm]" << std::endl; std::cerr << "[" << print_str << "] toe_angle = " << get_toe_angle() << "[deg]" << std::endl; std::cerr << "[" << print_str << "] heel_angle = " << get_heel_angle() << "[deg]" << std::endl; - std::cerr << "[" << print_str << "] use_toe_joint = " << (get_use_toe_joint()?"true":"false") << ", use_toe_heel_transition = " << (get_use_toe_heel_transition()?"true":"false") << std::endl; + std::cerr << "[" << print_str << "] use_toe_joint = " << (get_use_toe_joint()?"true":"false") << ", use_toe_heel_transition = " << (get_use_toe_heel_transition()?"true":"false") << ", use_toe_heel_auto_set = " << (get_use_toe_heel_auto_set()?"true":"false") << std::endl; std::vector tmp_ratio(get_NUM_TH_PHASES(), 0.0); get_toe_heel_phase_ratio(tmp_ratio); std::cerr << "[" << print_str << "] toe_heel_phase_ratio = ["; for (int i = 0; i < get_NUM_TH_PHASES(); i++) std::cerr << tmp_ratio[i] << " "; std::cerr << "]" << std::endl; std::cerr << "[" << print_str << "] optional_go_pos_finalize_footstep_num = " << optional_go_pos_finalize_footstep_num << ", overwritable_footstep_index_offset = " << overwritable_footstep_index_offset << std::endl; + std::cerr << "[" << print_str << "] default_stride_limitation_type = "; + if (default_stride_limitation_type == SQUARE) { + std::cerr << "SQUARE" << std::endl; + } else if (default_stride_limitation_type == CIRCLE) { + std::cerr << "CIRCLE" << std::endl; + } + thtc.print_param(print_str); }; }; } diff --git a/rtc/AutoBalancer/PreviewController.h b/rtc/AutoBalancer/PreviewController.h index ab3d888fa10..55ffa8cc8d0 100644 --- a/rtc/AutoBalancer/PreviewController.h +++ b/rtc/AutoBalancer/PreviewController.h @@ -5,7 +5,7 @@ #include #include #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" namespace rats { @@ -113,6 +113,7 @@ namespace rats }; // void update_zc(double zc); size_t get_delay () { return delay; }; + double get_preview_f (const size_t idx) { return f(idx); }; void get_refcog (double* ret) { ret[0] = x_k(0,0); @@ -165,6 +166,19 @@ namespace rats pz.clear(); qdata.clear(); }; + void set_preview_queue(const hrp::Vector3& pr, const std::vector& q, const size_t idx) + { + Eigen::Matrix tmpv; + tmpv(0,0) = pr(0); + tmpv(1,0) = pr(1); + p[idx] = tmpv; + pz[idx] = pr(2); + qdata[idx] = q; + }; + size_t get_preview_queue_size() + { + return p.size(); + }; void print_all_queue () { std::cerr << "(list "; @@ -263,6 +277,14 @@ namespace rats { preview_controller.remove_preview_queue(); }; + void set_preview_queue(const hrp::Vector3& pr, const std::vector& qdata, const size_t idx) + { + preview_controller.set_preview_queue(pr, qdata, idx); + } + size_t get_preview_queue_size() + { + return preview_controller.get_preview_queue_size(); + }; void print_all_queue () { preview_controller.print_all_queue(); @@ -274,6 +296,7 @@ namespace rats void get_current_refzmp (double* ret) { preview_controller.get_current_refzmp(ret);} //void get_current_qdata (double* ret) { preview_controller.get_current_qdata(ret);} size_t get_delay () { return preview_controller.get_delay(); }; + double get_preview_f (const size_t idx) { return preview_controller.get_preview_f(idx); }; }; } #endif /*PREVIEW_H_*/ diff --git a/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h b/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h new file mode 100644 index 00000000000..21716287868 --- /dev/null +++ b/rtc/AutoBalancer/SimpleFullbodyInverseKinematicsSolver.h @@ -0,0 +1,348 @@ +#ifndef SimpleFullbodyInverseKinematicsSolver_H +#define SimpleFullbodyInverseKinematicsSolver_H + +#include +#include "../ImpedanceController/JointPathEx.h" +#include "../ImpedanceController/RatsMatrix.h" +#include "AutoBalancerService_impl.h" + +// Class for Simple Fullbody Inverse Kinematics +// Input : target root pos and rot, target COG, target joint angles, target EE coords +// Output : joint angles +// Algorithm : Limb IK + move base +class SimpleFullbodyInverseKinematicsSolver +{ +protected: + // Robot model for IK + hrp::BodyPtr m_robot; + // Org (current) joint angles before IK + hrp::dvector qorg; + // IK fail checking + int ik_error_debug_print_freq; + std::string print_str; + bool has_ik_failed; +public: + // IK parameter for each limb + struct IKparam { + // IK target EE coords + hrp::Vector3 target_p0; + hrp::Matrix33 target_r0; + // EE offset, EE link + hrp::Vector3 localPos; + hrp::Matrix33 localR; + hrp::Link* target_link; + // IK solver and parameter + hrp::JointPathExPtr manip; + double avoid_gain, reference_gain; + // IK fail checking + size_t pos_ik_error_count, rot_ik_error_count; + // Solve ik or not + bool is_ik_enable; + // Name of parent link in limb + std::string parent_name; + // Limb length + double max_limb_length, limb_length_margin; + std::vector group_indices; + IKparam () + : avoid_gain(0.001), reference_gain(0.01), + pos_ik_error_count(0), rot_ik_error_count(0), + limb_length_margin(0.02) + { + }; + }; + std::map ikp; + // Used for ref joint angles overwrite before IK + std::vector overwrite_ref_ja_index_vec; + // IK targets and current? + hrp::dvector qrefv; + hrp::Vector3 target_root_p; + hrp::Matrix33 target_root_R; + hrp::Vector3 current_root_p; + hrp::Matrix33 current_root_R; + // IK params + double move_base_gain, ratio_for_vel; + // For IK fail checking + double pos_ik_thre, rot_ik_thre; + // For limb stretch avoidance + double d_root_height, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], m_dt; + bool use_limb_stretch_avoidance; + + SimpleFullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt) + : m_robot(_robot), + ik_error_debug_print_freq(static_cast(0.2/_dt)), // once per 0.2 [s] + has_ik_failed(false), + overwrite_ref_ja_index_vec(), ikp(), + move_base_gain(0.8), ratio_for_vel(1.0), + pos_ik_thre(0.5*1e-3), // [m] + rot_ik_thre((1e-2)*M_PI/180.0), // [rad] + print_str(_print_str), m_dt(_dt), + use_limb_stretch_avoidance(false), limb_stretch_avoidance_time_const(1.5) + { + qorg.resize(m_robot->numJoints()); + qrefv.resize(m_robot->numJoints()); + limb_stretch_avoidance_vlimit[0] = -1000 * 1e-3 * _dt; // lower limit + limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * _dt; // upper limit + }; + ~SimpleFullbodyInverseKinematicsSolver () {}; + + void initializeInterlockingJoints (std::vector > & interlocking_joints) + { + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + std::cerr << "[" << print_str << "] Interlocking Joints for [" << it->first << "]" << std::endl; + it->second.manip->setInterlockingJointPairIndices(interlocking_joints, print_str); + } + }; + void storeCurrentParameters() + { + current_root_p = m_robot->rootLink()->p; + current_root_R = m_robot->rootLink()->R; + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + qorg[i] = m_robot->joint(i)->q; + } + }; + void revertRobotStateToCurrent () + { + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if (it->second.is_ik_enable) { + for ( unsigned int j = 0; j < it->second.manip->numJoints(); j++ ){ + int i = it->second.manip->joint(j)->jointId; + m_robot->joint(i)->q = qorg[i]; + } + } + } + m_robot->rootLink()->p = current_root_p; + m_robot->rootLink()->R = current_root_R; + m_robot->calcForwardKinematics(); + }; + void setReferenceJointAngles () + { + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + qrefv[i] = m_robot->joint(i)->q; + } + }; + // Solve fullbody IK using limb IK + void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool is_transition) + { + hrp::Vector3 dif_cog(ratio_for_vel*_dif_cog); + dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2); + m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog; + m_robot->rootLink()->R = target_root_R; + // Avoid limb stretch + { + std::vector tmp_p; + std::vector tmp_name; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if (it->first.find("leg") != std::string::npos) { + tmp_p.push_back(it->second.target_p0); + tmp_name.push_back(it->first); + } + } + limbStretchAvoidanceControl(tmp_p, tmp_name); + } + // Overwrite by ref joint angle + for (size_t i = 0; i < overwrite_ref_ja_index_vec.size(); i++) { + m_robot->joint(overwrite_ref_ja_index_vec[i])->q = qrefv[overwrite_ref_ja_index_vec[i]]; + } + m_robot->calcForwardKinematics(); + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if (it->second.is_ik_enable) solveLimbIK (it->second, it->first, ratio_for_vel, is_transition); + } + }; + void solveSimpleFullbodyIKLoop (const hrp::Vector3& _dif_cog, const bool is_transition) { + hrp::Vector3 dif_cog(ratio_for_vel*_dif_cog); + dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2) - d_root_height; + m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog; + m_robot->calcForwardKinematics(); + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + if (it->second.is_ik_enable) solveLimbIK (it->second, it->first, ratio_for_vel, is_transition); + } + } + // Solve limb IK + bool solveLimbIK (IKparam& param, const std::string& limb_name, const double ratio_for_vel, const bool is_transition) + { + param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, ratio_for_vel, + param.localPos, param.localR); + checkIKTracking(param, limb_name, is_transition); + return true; + } + // IK fail check + void checkIKTracking (IKparam& param, const std::string& limb_name, const bool is_transition) + { + hrp::Vector3 vel_p, vel_r; + vel_p = param.target_p0 - (param.target_link->p + param.target_link->R * param.localPos); + rats::difference_rotation(vel_r, (param.target_link->R * param.localR), param.target_r0); + if (vel_p.norm() > pos_ik_thre && is_transition) { + if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) { + std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl; + } + param.pos_ik_error_count++; + has_ik_failed = true; + } else { + param.pos_ik_error_count = 0; + } + if (vel_r.norm() > rot_ik_thre && is_transition) { + if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) { + std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl; + } + param.rot_ik_error_count++; + has_ik_failed = true; + } else { + param.rot_ik_error_count = 0; + } + }; + // Reset IK fail params + void resetIKFailParam() { + has_ik_failed = false; + for ( std::map::iterator it = ikp.begin(); it != ikp.end(); it++ ) { + it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0; + } + } + // Get IKparam + void getIKParam (std::vector& ee_vec, _CORBA_Unbounded_Sequence& ik_limb_parameters) + { + ik_limb_parameters.length(ee_vec.size()); + for (size_t i = 0; i < ee_vec.size(); i++) { + IKparam& param = ikp[ee_vec[i]]; + OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i]; + ilp.ik_optional_weight_vector.length(param.manip->numJoints()); + std::vector ov; + ov.resize(param.manip->numJoints()); + param.manip->getOptionalWeightVector(ov); + for (size_t j = 0; j < param.manip->numJoints(); j++) { + ilp.ik_optional_weight_vector[j] = ov[j]; + } + ilp.sr_gain = param.manip->getSRGain(); + ilp.avoid_gain = param.avoid_gain; + ilp.reference_gain = param.reference_gain; + ilp.manipulability_limit = param.manip->getManipulabilityLimit(); + } + }; + // Set IKparam + void setIKParam (std::vector& ee_vec, const _CORBA_Unbounded_Sequence& ik_limb_parameters) + { + std::cerr << "[" << print_str << "] IK limb parameters" << std::endl; + bool is_ik_limb_parameter_valid_length = true; + if (ik_limb_parameters.length() != ee_vec.size()) { + is_ik_limb_parameter_valid_length = false; + std::cerr << "[" << print_str << "] ik_limb_parameters invalid length! Cannot be set. (input = " << ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl; + } else { + for (size_t i = 0; i < ee_vec.size(); i++) { + if (ikp[ee_vec[i]].manip->numJoints() != ik_limb_parameters[i].ik_optional_weight_vector.length()) + is_ik_limb_parameter_valid_length = false; + } + if (is_ik_limb_parameter_valid_length) { + for (size_t i = 0; i < ee_vec.size(); i++) { + IKparam& param = ikp[ee_vec[i]]; + const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i]; + std::vector ov; + ov.resize(param.manip->numJoints()); + for (size_t j = 0; j < param.manip->numJoints(); j++) { + ov[j] = ilp.ik_optional_weight_vector[j]; + } + param.manip->setOptionalWeightVector(ov); + param.manip->setSRGain(ilp.sr_gain); + param.avoid_gain = ilp.avoid_gain; + param.reference_gain = ilp.reference_gain; + param.manip->setManipulabilityLimit(ilp.manipulability_limit); + } + } else { + std::cerr << "[" << print_str << "] ik_optional_weight_vector invalid length! Cannot be set. (input = ["; + for (size_t i = 0; i < ee_vec.size(); i++) { + std::cerr << ik_limb_parameters[i].ik_optional_weight_vector.length() << ", "; + } + std::cerr << "], desired = ["; + for (size_t i = 0; i < ee_vec.size(); i++) { + std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", "; + } + std::cerr << "])" << std::endl; + } + } + if (is_ik_limb_parameter_valid_length) { + printIKparam(ee_vec); + } + }; + // Avoid limb stretch + void limbStretchAvoidanceControl (const std::vector& target_p, const std::vector& target_name) + { + m_robot->calcForwardKinematics(); + double tmp_d_root_height = 0.0, prev_d_root_height = d_root_height; + if (use_limb_stretch_avoidance) { + for (size_t i = 0; i < target_p.size(); i++) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(ikp[target_name[i]].parent_name); + hrp::Vector3 rel_target_p = target_p[i] - parent_link->p; // position from parent to target link (world frame) + double limb_length_limitation = ikp[target_name[i]].max_limb_length - ikp[target_name[i]].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - rel_target_p(0) * rel_target_p(0) - rel_target_p(1) * rel_target_p(1); + if (rel_target_p.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_root_height = std::min(tmp_d_root_height, rel_target_p(2) + std::sqrt(tmp)); + } + } + // Change root link height depending on limb length + d_root_height = tmp_d_root_height == 0.0 ? (- 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height) : tmp_d_root_height; + } else { + d_root_height = - 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height; + } + d_root_height = vlimit(d_root_height, prev_d_root_height + limb_stretch_avoidance_vlimit[0], prev_d_root_height + limb_stretch_avoidance_vlimit[1]); + m_robot->rootLink()->p(2) += d_root_height; + }; + double vlimit(const double value, const double llimit_value, const double ulimit_value) + { + if (value > ulimit_value) { + return ulimit_value; + } else if (value < llimit_value) { + return llimit_value; + } + return value; + }; + // Set parameter + void printParam () const + { + std::cerr << "[" << print_str << "] move_base_gain = " << move_base_gain << std::endl; + std::cerr << "[" << print_str << "] pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl; + }; + // Set IKparam + void printIKparam (std::vector& ee_vec) + { + std::cerr << "[" << print_str << "] ik_optional_weight_vectors = "; + for (size_t i = 0; i < ee_vec.size(); i++) { + IKparam& param = ikp[ee_vec[i]]; + std::vector ov; + ov.resize(param.manip->numJoints()); + param.manip->getOptionalWeightVector(ov); + std::cerr << "["; + for (size_t j = 0; j < param.manip->numJoints(); j++) { + std::cerr << ov[j] << " "; + } + std::cerr << "]"; + } + std::cerr << std::endl; + std::cerr << "[" << print_str << "] sr_gains = ["; + for (size_t i = 0; i < ee_vec.size(); i++) { + std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << print_str << "] avoid_gains = ["; + for (size_t i = 0; i < ee_vec.size(); i++) { + std::cerr << ikp[ee_vec[i]].avoid_gain << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << print_str << "] reference_gains = ["; + for (size_t i = 0; i < ee_vec.size(); i++) { + std::cerr << ikp[ee_vec[i]].reference_gain << ", "; + } + std::cerr << "]" << std::endl; + std::cerr << "[" << print_str << "] manipulability_limits = ["; + for (size_t i = 0; i < ee_vec.size(); i++) { + std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", "; + } + std::cerr << "]" << std::endl; + }; + hrp::Vector3 getEndEffectorPos(const std::string& limb_name){ + return ikp[limb_name].target_link->p + ikp[limb_name].target_link->R * ikp[limb_name].localPos; + } + hrp::Matrix33 getEndEffectorRot(const std::string& limb_name){ + return ikp[limb_name].target_link->R * ikp[limb_name].localR; + } +}; + +#endif // SimpleFullbodyInverseKinematicsSolver_H diff --git a/rtc/AutoBalancer/Stabilizer.cpp b/rtc/AutoBalancer/Stabilizer.cpp new file mode 100644 index 00000000000..dc12f5cec8d --- /dev/null +++ b/rtc/AutoBalancer/Stabilizer.cpp @@ -0,0 +1,1855 @@ +// -*- C++ -*- +/*! + * @file Stabilizer.cpp + * @brief stabilizer filter + * $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include +#include "Stabilizer.h" +#include "hrpsys/util/VectorConvert.h" +#include +#include + +typedef coil::Guard Guard; + +#ifndef deg2rad +#define deg2rad(x) ((x) * M_PI / 180.0) +#endif +#ifndef rad2deg +#define rad2deg(rad) (rad * 180 / M_PI) +#endif + +#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 ) +#define DEBUGP2 (loop%10==0) + +void Stabilizer::initStabilizer(const RTC::Properties& prop, const size_t& num) +{ + std::vector > interlocking_joints; + readInterlockingJointsParamFromProperties(interlocking_joints, m_robot, prop["interlocking_joints"], std::string(print_str)); + if (interlocking_joints.size() > 0) { + for (size_t i = 0; i < jpe_v.size(); i++) { + std::cerr << "[" << print_str << "] Interlocking Joints for [" << stikp[i].ee_name << "]" << std::endl; + jpe_v[i]->setInterlockingJointPairIndices(interlocking_joints, std::string(print_str)); + } + } + + // parameters for TPCC + act_zmp = hrp::Vector3::Zero(); + for (int i = 0; i < 2; i++) { + k_tpcc_p[i] = 0.2; + k_tpcc_x[i] = 4.0; + k_brot_p[i] = 0.1; + k_brot_tc[i] = 1.5; + } + // parameters for EEFM + double k_ratio = 0.9; + for (int i = 0; i < 2; i++) { + eefm_k1[i] = -1.41429*k_ratio; + eefm_k2[i] = -0.404082*k_ratio; + eefm_k3[i] = -0.18*k_ratio; + eefm_body_attitude_control_gain[i] = 0.5; + eefm_body_attitude_control_time_const[i] = 1e5; + } + for (size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + hrp::Link* root = m_robot->link(ikp.target_name); + ikp.eefm_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5); + ikp.eefm_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5); + ikp.eefm_rot_compensation_limit = deg2rad(10.0); + ikp.eefm_swing_rot_spring_gain = hrp::Vector3(0.0, 0.0, 0.0); + ikp.eefm_swing_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5); + ikp.eefm_pos_damping_gain = hrp::Vector3(3500*10, 3500*10, 3500); + ikp.eefm_pos_time_const_support = hrp::Vector3(1.5, 1.5, 1.5); + ikp.eefm_pos_compensation_limit = 0.025; + ikp.eefm_swing_pos_spring_gain = hrp::Vector3(0.0, 0.0, 0.0); + ikp.eefm_swing_pos_time_const = hrp::Vector3(1.5, 1.5, 1.5); + ikp.eefm_ee_moment_limit = hrp::Vector3(1e4, 1e4, 1e4); // Default limit [Nm] is too large. Same as no limit. + ikp.touchoff_remain_time = 0.0; + if (ikp.ee_name.find("leg") == std::string::npos) { // Arm default + ikp.eefm_ee_forcemoment_distribution_weight = Eigen::Matrix::Zero(); + } else { // Leg default + for (size_t j = 0; j < 3; j++) { + ikp.eefm_ee_forcemoment_distribution_weight[j] = 1; // Force + ikp.eefm_ee_forcemoment_distribution_weight[j+3] = 1e-2; // Moment + } + } + ikp.max_limb_length = 0.0; + while (!root->isRoot()) { + ikp.max_limb_length += root->b.norm(); + ikp.parent_name = root->name; + root = root->parent; + } + ikp.limb_length_margin = 0.13; + ikp.support_time = 0.0; + } + eefm_swing_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5); + eefm_swing_pos_damping_gain = hrp::Vector3(33600, 33600, 7000); + eefm_pos_time_const_swing = 0.08; + eefm_pos_transition_time = 0.01; + eefm_pos_margin_time = 0.02; + eefm_zmp_delay_time_const[0] = eefm_zmp_delay_time_const[1] = 0.055; + //eefm_leg_inside_margin = 0.065; // [m] + //eefm_leg_front_margin = 0.05; + //eefm_leg_rear_margin = 0.05; + //fm_wrench_alpha_blending = 1.0; // fz_alpha + eefm_gravitational_acceleration = 9.80665; // [m/s^2] + cop_check_margin = 20.0*1e-3; // [m] + cp_check_margin.resize(4, 30*1e-3); // [m] + cp_offset = hrp::Vector3(0.0, 0.0, 0.0); // [m] + tilt_margin.resize(2, 30 * M_PI / 180); // [rad] + contact_decision_threshold = 50; // [N] + eefm_use_force_difference_control = true; + eefm_use_swing_damping = false; + eefm_swing_damping_force_thre.resize(3, 300); + eefm_swing_damping_moment_thre.resize(3, 15); + initial_cp_too_large_error = true; + is_walking = false; + is_single_walking = false; + is_estop_while_walking = false; + sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0); + use_limb_stretch_avoidance = false; + use_zmp_truncation = false; + limb_stretch_avoidance_time_const = 1.5; + limb_stretch_avoidance_vlimit[0] = -100 * 1e-3 * dt; // lower limit + limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * dt; // upper limit + root_rot_compensation_limit[0] = root_rot_compensation_limit[1] = deg2rad(90.0); + detection_count_to_air = static_cast(0.0 / dt); + transition_interpolator = new interpolator(1, dt, interpolator::HOFFARBIB, 1); + transition_interpolator->setName(std::string(print_str)+" transition_interpolator"); + is_foot_touch.resize(stikp.size(), true); + touchdown_d_pos.resize(stikp.size(), hrp::Vector3::Zero()); + touchdown_d_rpy.resize(stikp.size(), hrp::Vector3::Zero()); + prev_ref_zmp = hrp::Vector3::Zero(); + prev_ref_cog = hrp::Vector3::Zero(); + act_cogvel = hrp::Vector3::Zero(); + prev_act_foot_origin_rot = hrp::Matrix33::Identity(); + prev_act_foot_origin_pos = hrp::Vector3::Zero(); + swing2landing_transition_time = 0.05; + landing_phase_time = 0.1; + landing2support_transition_time = 0.5; + support_phase_min_time = 0.1; + support2swing_transition_time = 0.05; + use_force_sensor = true; + is_reset_torque = false; + is_after_walking = false; + after_walking_interpolator = new interpolator(1, dt, interpolator::HOFFARBIB, 1); + after_walking_interpolator->setName(std::string(print_str)+" after_walking_interpolator"); + use_footguided_stabilizer = true; + footguided_balance_time_const = 0.4; // [s] + + // Check is legged robot or not + is_legged_robot = false; + for (size_t i = 0; i < stikp.size(); i++) { + if (stikp[i].ee_name.find("leg") == std::string::npos) continue; + hrp::Sensor* sen= m_robot->sensor(stikp[i].sensor_name); + if ( sen != NULL ) is_legged_robot = true; + } + is_emergency_motion = is_emergency_step = is_emergency = false; + reset_emergency_flag = false; + + // m_tau.data.length(m_robot->numJoints()); + transition_joint_q.resize(m_robot->numJoints()); + qorg.resize(m_robot->numJoints()); + qrefv.resize(m_robot->numJoints()); + transition_count = 0; + loop = 0; + m_is_falling_counter = 0; + is_air_counter = 0; + total_mass = m_robot->totalMass(); + ref_zmp_aux = hrp::Vector3::Zero(); + + qRef.resize(m_robot->numJoints()); + qCurrent.resize(m_robot->numJoints()); + diff_q.resize(m_robot->numJoints()); + qRefSeq.resize(m_robot->numJoints()); + controlSwingSupportTime.resize(num, 1.0); + copInfo.resize(num*3, 1.0); // nx, ny, fz for each end-effectors + // m_actContactStates.data.length(m_contactStates.data.length()); + for (size_t i = 0; i < num; i++) { + target_ee_p.push_back(hrp::Vector3::Zero()); + target_ee_R.push_back(hrp::Matrix33::Identity()); + act_ee_p.push_back(hrp::Vector3::Zero()); + act_ee_R.push_back(hrp::Matrix33::Identity()); + projected_normal.push_back(hrp::Vector3::Zero()); + act_force.push_back(hrp::Vector3::Zero()); + ref_force.push_back(hrp::Vector3::Zero()); + ref_moment.push_back(hrp::Vector3::Zero()); + prev_act_force.push_back(hrp::Vector3::Zero()); + ref_contact_states.push_back(true); + prev_ref_contact_states.push_back(true); + // m_actContactStates.data[i] = false; + act_contact_states.push_back(false); + prev_act_contact_states.push_back(false); + toeheel_ratio.push_back(1.0); + } + transition_time = 2.0; + foot_origin_offset[0] = hrp::Vector3::Zero(); + foot_origin_offset[1] = hrp::Vector3::Zero(); + + act_cogvel_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(4.0, dt, hrp::Vector3::Zero())); // [Hz] + + // // for debug output + // m_originRefZmp.data.x = m_originRefZmp.data.y = m_originRefZmp.data.z = 0.0; + // m_originRefCog.data.x = m_originRefCog.data.y = m_originRefCog.data.z = 0.0; + // m_originRefCogVel.data.x = m_originRefCogVel.data.y = m_originRefCogVel.data.z = 0.0; + // m_originNewZmp.data.x = m_originNewZmp.data.y = m_originNewZmp.data.z = 0.0; + // m_originActZmp.data.x = m_originActZmp.data.y = m_originActZmp.data.z = 0.0; + // m_originActCog.data.x = m_originActCog.data.y = m_originActCog.data.z = 0.0; + // m_originActCogVel.data.x = m_originActCogVel.data.y = m_originActCogVel.data.z = 0.0; + // m_allRefWrench.data.length(stikp.size() * 6); // 6 is wrench dim + // m_allEEComp.data.length(stikp.size() * 6); // 6 is pos+rot dim + // m_debugData.data.length(1); m_debugData.data[0] = 0.0; + + szd = new SimpleZMPDistributor(dt); + std::vector > support_polygon_vec; + for (size_t i = 0; i < stikp.size(); i++) { + support_polygon_vec.push_back(std::vector(1,Eigen::Vector2d::Zero())); + } + szd->set_vertices(support_polygon_vec); + + rel_ee_pos.reserve(stikp.size()); + rel_ee_rot.reserve(stikp.size()); + rel_ee_name.reserve(stikp.size()); + rel_ee_rot_for_ik.reserve(stikp.size()); + + hrp::Sensor* sen = m_robot->sensor("gyrometer"); + if (sen == NULL) { + std::cerr << "[" << print_str << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl; + } +} + +void Stabilizer::execStabilizer() +{ + loop++; + + if (is_legged_robot) { + getTargetParameters(); + getActualParametersForST(); + calcStateForEmergencySignal(); + switch (control_mode) { + case MODE_IDLE: + if (!is_reset_torque) { + if ( joint_control_mode == OpenHRP::RobotHardwareService::TORQUE ) { + double tmp_time = 3.0; + m_robotHardwareService0->setServoPGainPercentageWithTime("all",100,tmp_time); + m_robotHardwareService0->setServoDGainPercentageWithTime("all",100,tmp_time); + usleep(tmp_time * 1e6); + m_robotHardwareService0->setServoTorqueGainPercentage("all",0); + is_reset_torque = true; + } + } + break; + case MODE_AIR: + if ( transition_count == 0 && on_ground ) sync_2_st(); + break; + case MODE_ST: + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + calcEEForceMomentControl(); + } else { + calcTPCC(); + } + if ( transition_count == 0 && !on_ground ) { + if (is_air_counter < detection_count_to_air) ++is_air_counter; + else control_mode = MODE_SYNC_TO_AIR; + } else is_air_counter = 0; + break; + case MODE_SYNC_TO_IDLE: + sync_2_idle(); + control_mode = MODE_IDLE; + break; + case MODE_SYNC_TO_AIR: + sync_2_idle(); + control_mode = MODE_AIR; + break; + } + copy (ref_contact_states.begin(), ref_contact_states.end(), prev_ref_contact_states.begin()); + copy (act_contact_states.begin(), act_contact_states.end(), prev_act_contact_states.begin()); + if (!transition_interpolator->isEmpty()) { + for (int i = 0; i < m_robot->numJoints(); i++ ) { + diff_q[i] = m_robot->joint(i)->q - qRef[i]; + } + } + getCurrentParameters(); + } +} + +void Stabilizer::getCurrentParameters () +{ + current_root_p = m_robot->rootLink()->p; + current_root_R = m_robot->rootLink()->R; + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + qorg[i] = m_robot->joint(i)->q; + } +} + +void Stabilizer::getTargetParameters () +{ + // Reference world frame => + // update internal robot model + if ( transition_count == 0 ) { + transition_smooth_gain = 1.0; + } else { + double max_transition_count = calcMaxTransitionCount(); + transition_smooth_gain = 1/(1+exp(-9.19*(((max_transition_count - std::fabs(transition_count)) / max_transition_count) - 0.5))); + } + if (transition_count > 0) { + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = ( qRef[i] - transition_joint_q[i] ) * transition_smooth_gain + transition_joint_q[i]; + } + } else { + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = qRef[i]; + } + } + if (!transition_interpolator->isEmpty()) { + double tmp_ratio; + transition_interpolator->get(&tmp_ratio, true); + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = qRef[i] + tmp_ratio * diff_q[i]; + } + } + if ( transition_count < 0 ) { + transition_count++; + } else if ( transition_count > 0 ) { + if ( transition_count == 1 ) { + std::cerr << "[" << print_str << "] [" << "] Move to MODE_IDLE" << std::endl; + reset_emergency_flag = true; + } + transition_count--; + } + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + qrefv[i] = m_robot->joint(i)->q; + } + m_robot->rootLink()->p = basePos; + target_root_p = m_robot->rootLink()->p; + target_root_R = hrp::rotFromRpy(baseRpy); + m_robot->rootLink()->R = target_root_R; + m_robot->calcForwardKinematics(); + ref_zmp = m_robot->rootLink()->R * zmpRef + m_robot->rootLink()->p; // base frame -> world frame + hrp::Vector3 foot_origin_pos; + hrp::Matrix33 foot_origin_rot; + calcFootOriginCoords (foot_origin_pos, foot_origin_rot); + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + // apply inverse system + hrp::Vector3 tmp_ref_zmp = ref_zmp + eefm_zmp_delay_time_const[0] * (ref_zmp - prev_ref_zmp) / dt; + prev_ref_zmp = ref_zmp; + ref_zmp = tmp_ref_zmp; + } + ref_cog = m_robot->calcCM(); + ref_total_force = hrp::Vector3::Zero(); + ref_total_moment = hrp::Vector3::Zero(); // Total moment around reference ZMP tmp + ref_total_foot_origin_moment = hrp::Vector3::Zero(); + for (size_t i = 0; i < stikp.size(); i++) { + hrp::Link* target = m_robot->link(stikp[i].target_name); + //target_ee_p[i] = target->p + target->R * stikp[i].localCOPPos; + target_ee_p[i] = target->p + target->R * stikp[i].localp; + target_ee_R[i] = target->R * stikp[i].localR; + stikp[i].ref_force = hrp::Vector3(ref_wrenches[i][0], ref_wrenches[i][1], ref_wrenches[i][2]); + stikp[i].ref_moment = hrp::Vector3(ref_wrenches[i][3], ref_wrenches[i][4], ref_wrenches[i][5]); + ref_force[i] = foot_origin_rot * stikp[i].ref_force; + ref_moment[i] = foot_origin_rot * stikp[i].ref_moment; + ref_total_force += ref_force[i]; + // Force/moment diff control + ref_total_moment += (target_ee_p[i]-ref_zmp).cross(ref_force[i]); + // Force/moment control + // ref_total_moment += (target_ee_p[i]-ref_zmp).cross(hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2])) + // + hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5]); + if (is_feedback_control_enable[i]) { + ref_total_foot_origin_moment += (target_ee_p[i]-foot_origin_pos).cross(ref_force[i]) + ref_moment[i]; + } + } + // <= Reference world frame + + // Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST) because the coordinates for ref_cog differs among st algorithms. + if (transition_count == (-1 * calcMaxTransitionCount() + 1)) { // max transition count. In MODE_IDLE => MODE_ST, transition_count is < 0 and upcounter. "+ 1" is upcount at the beginning of this function. + prev_ref_cog = ref_cog; + std::cerr << "[" << print_str << "] Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST)." << std::endl; + } + + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + // Reference foot_origin frame => + // initialize for new_refzmp + new_refzmp = ref_zmp; + rel_cog = m_robot->rootLink()->R.transpose() * (ref_cog-m_robot->rootLink()->p); + ref_cogvel = (ref_cog - prev_ref_cog)/dt; + prev_ref_cog = ref_cog; + // convert world (current-tmp) => local (foot_origin) + zmp_origin_off = ref_zmp(2) - foot_origin_pos(2); + ref_zmp = foot_origin_rot.transpose() * (ref_zmp - foot_origin_pos); + ref_cog = foot_origin_rot.transpose() * (ref_cog - foot_origin_pos); + new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos); + ref_cogvel = foot_origin_rot.transpose() * ref_cogvel; + ref_foot_origin_rot = foot_origin_rot; + ref_foot_origin_pos = foot_origin_pos; + for (size_t i = 0; i < stikp.size(); i++) { + stikp[i].target_ee_diff_p = foot_origin_rot.transpose() * (target_ee_p[i] - foot_origin_pos); + stikp[i].ref_theta = Eigen::AngleAxisd(foot_origin_rot.transpose() * target_ee_R[i]); + ref_force[i] = foot_origin_rot.transpose() * ref_force[i]; + ref_moment[i] = foot_origin_rot.transpose() * ref_moment[i]; + } + ref_total_foot_origin_moment = foot_origin_rot.transpose() * ref_total_foot_origin_moment; + ref_total_force = foot_origin_rot.transpose() * ref_total_force; + ref_total_moment = foot_origin_rot.transpose() * ref_total_moment; + target_foot_origin_rot = foot_origin_rot; + // capture point + ref_cp = ref_cog + ref_cogvel / std::sqrt((eefm_gravitational_acceleration - total_external_force_z/total_mass) / (ref_cog - ref_zmp)(2)); + rel_ref_cp = hrp::Vector3(ref_cp(0), ref_cp(1), ref_zmp(2)); + rel_ref_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_ref_cp) - m_robot->rootLink()->p); + sbp_cog_offset = foot_origin_rot.transpose() * sbp_cog_offset; + // <= Reference foot_origin frame + } else { + ref_cogvel = (ref_cog - prev_ref_cog)/dt; + } // st_algorithm == OpenHRP::AutoBalancerService::EEFM + // Calc swing support limb gain param + calcSwingSupportLimbGain(); +} + +void Stabilizer::getActualParameters () +{ + // Actual world frame => + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + // update by current joint angles + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = qCurrent[i]; + } + // tempolary + m_robot->rootLink()->p = hrp::Vector3::Zero(); + m_robot->calcForwardKinematics(); + hrp::Sensor* sen = m_robot->sensor("gyrometer"); + hrp::Matrix33 senR = sen->link->R * sen->localR; + hrp::Matrix33 act_Rs(hrp::rotFromRpy(rpy)); + //hrp::Matrix33 act_Rs(hrp::rotFromRpy(m_rpy.data.r*0.5, m_rpy.data.p*0.5, m_rpy.data.y*0.5)); + act_root_R = m_robot->rootLink()->R = act_Rs * (senR.transpose() * m_robot->rootLink()->R); + m_robot->calcForwardKinematics(); + act_base_rpy = hrp::rpyFromRot(m_robot->rootLink()->R); + calcFootOriginCoords (foot_origin_pos, foot_origin_rot); + } else { + for ( int i = 0; i < m_robot->numJoints(); i++ ) { + m_robot->joint(i)->q = qorg[i]; + } + m_robot->rootLink()->p = current_root_p; + m_robot->rootLink()->R = current_root_R; + m_robot->calcForwardKinematics(); + } + // cog + act_cog = m_robot->calcCM(); + // zmp + on_ground = false; + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + on_ground = calcZMP(act_zmp, zmp_origin_off+foot_origin_pos(2), foot_origin_rot); + } else { + on_ground = calcZMP(act_zmp, ref_zmp(2), foot_origin_rot); + } + // set actual contact states + for (size_t i = 0; i < stikp.size(); i++) { + std::string limb_name = stikp[i].ee_name; + size_t idx = contact_states_index_map[limb_name]; + act_contact_states[idx] = (use_force_sensor || limb_name.find("leg") == std::string::npos) ? isContact(idx) : ref_contact_states[idx]; + // m_actContactStates.data[idx] = act_contact_states[idx]; + } + // <= Actual world frame + + // convert absolute (in st) -> root-link relative + rel_act_zmp = m_robot->rootLink()->R.transpose() * (act_zmp - m_robot->rootLink()->p); + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + // Actual foot_origin frame => + act_zmp = foot_origin_rot.transpose() * (act_zmp - foot_origin_pos); + act_cog = foot_origin_rot.transpose() * (act_cog - foot_origin_pos); + if (!use_force_sensor) { + on_ground = true; + act_zmp = ref_zmp; + } + //act_cogvel = foot_origin_rot.transpose() * act_cogvel; + if ((foot_origin_pos - prev_act_foot_origin_pos).norm() > 1e-2) { // assume that origin_pos changes more than 1cm when contact states change + act_cogvel = (foot_origin_rot.transpose() * prev_act_foot_origin_rot) * act_cogvel; + } else { + if (act_contact_states[contact_states_index_map["rleg"]] || act_contact_states[contact_states_index_map["lleg"]]) { // on ground + act_cogvel = (act_cog - prev_act_cog)/dt; + } else if (prev_act_contact_states[contact_states_index_map["rleg"]] || prev_act_contact_states[contact_states_index_map["lleg"]]) { // take off + jump_time_count = 1; + jump_initial_velocity = act_cogvel(2); + act_cogvel(2) = jump_initial_velocity - eefm_gravitational_acceleration * jump_time_count * dt; + } else { // jumping + jump_time_count++; + act_cogvel(2) = jump_initial_velocity - eefm_gravitational_acceleration * jump_time_count * dt; + } + } + act_cogvel = act_cogvel_filter->passFilter(act_cogvel); + prev_act_cog = act_cog; + //act_root_rot = m_robot->rootLink()->R; + for (size_t i = 0; i < stikp.size(); i++) { + hrp::Link* target = m_robot->link(stikp[i].target_name); + //hrp::Vector3 act_ee_p = target->p + target->R * stikp[i].localCOPPos; + hrp::Vector3 _act_ee_p = target->p + target->R * stikp[i].localp; + act_ee_p[i] = foot_origin_rot.transpose() * (_act_ee_p - foot_origin_pos); + act_ee_R[i] = foot_origin_rot.transpose() * (target->R * stikp[i].localR); + } + // capture point + act_cp = act_cog + act_cogvel / std::sqrt((eefm_gravitational_acceleration - total_external_force_z/total_mass) / (act_cog - act_zmp)(2)); + rel_act_cp = hrp::Vector3(act_cp(0), act_cp(1), act_zmp(2)); + rel_act_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_act_cp) - m_robot->rootLink()->p); + // <= Actual foot_origin frame + } + // calc cmp + hrp::Vector3 total_force = hrp::Vector3::Zero(); + for (size_t i = 0; i < stikp.size(); i++) { // TODO remove duplicatation in getActualParametersForST + STIKParam& ikp = stikp[i]; + if (!is_feedback_control_enable[i]) continue; + hrp::Sensor* sensor = m_robot->sensor(ikp.sensor_name); + total_force += foot_origin_rot.transpose() * ((sensor->link->R * sensor->localR) * hrp::Vector3(wrenches[i][0], wrenches[i][1], wrenches[i][2])); + } + double tmp_k = (act_cog - act_zmp)(2) / total_force(2); + act_cmp.head(2) = (act_cog - tmp_k * total_force).head(2); + act_cmp(2) = act_zmp(2); +} + +void Stabilizer::getActualParametersForST () +{ + // Actual world frame => + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + // update by current joint angles + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = qCurrent[i]; + } + // tempolary + m_robot->rootLink()->p = hrp::Vector3::Zero(); + m_robot->rootLink()->R = act_root_R; + m_robot->calcForwardKinematics(); + } else { + for ( int i = 0; i < m_robot->numJoints(); i++ ) { + m_robot->joint(i)->q = qorg[i]; + } + m_robot->rootLink()->p = current_root_p; + m_robot->rootLink()->R = current_root_R; + m_robot->calcForwardKinematics(); + } + + if (st_algorithm != OpenHRP::AutoBalancerService::TPCC) { + // Actual world frame => + // new ZMP calculation + // Kajita's feedback law + // Basically Equation (26) in the paper [1]. + new_refzmp = foot_origin_rot * new_refzmp + foot_origin_pos; // = abs_ref_zmp + if (!is_walking || !use_act_states) { + if (use_footguided_stabilizer && use_act_states) { // guided_stabilzier is not valid while walking + double omega = std::sqrt((eefm_gravitational_acceleration - total_external_force_z/total_mass) / (act_cog - act_zmp)(2)); + hrp::Vector3 act_xcp = (foot_origin_pos + foot_origin_rot * (act_cp + sbp_cog_offset)) - new_refzmp; + hrp::Vector3 dxsp = hrp::Vector3::Zero(); // stay still + new_refzmp += transition_smooth_gain * 2 * (act_xcp - std::exp(- omega * footguided_balance_time_const) * dxsp) / (1 - std::exp(-2 * omega * footguided_balance_time_const)); + } else { // kajita st + hrp::Vector3 dcog=foot_origin_rot * (ref_cog - act_cog); + hrp::Vector3 dcogvel=foot_origin_rot * (ref_cogvel - act_cogvel); + hrp::Vector3 dzmp=foot_origin_rot * (ref_zmp - act_zmp); + for (size_t i = 0; i < 2; i++) { + new_refzmp(i) += eefm_k1[i] * transition_smooth_gain * dcog(i) + eefm_k2[i] * transition_smooth_gain * dcogvel(i) + eefm_k3[i] * transition_smooth_gain * dzmp(i) + ref_zmp_aux(i); + } + } + } + if (use_act_states && !use_footguided_stabilizer) { + if (is_walking) { + is_after_walking = true; + if (!after_walking_interpolator->isEmpty()) after_walking_interpolator->clear(); + after_walking_refzmp = new_refzmp; + } else { + if (is_after_walking) { + double tmp_ratio = 1.0; + after_walking_interpolator->set(&tmp_ratio); + tmp_ratio = 0.0; + after_walking_interpolator->setGoal(&tmp_ratio, 0.01, true); + is_after_walking = false; + } + if (!after_walking_interpolator->isEmpty()) { + double tmp_ratio; + after_walking_interpolator->get(&tmp_ratio, true); + new_refzmp = tmp_ratio * after_walking_refzmp + (1.0 - tmp_ratio) * new_refzmp; + } + } + } + if (DEBUGP) { + // All state variables are foot_origin coords relative + std::cerr << "[" << print_str << "] state values" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_cog = " << hrp::Vector3(ref_cog*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) + << ", act_cog = " << hrp::Vector3(act_cog*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_cogvel = " << hrp::Vector3(ref_cogvel*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) + << ", act_cogvel = " << hrp::Vector3(act_cogvel*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm/s]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_zmp = " << hrp::Vector3(ref_zmp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) + << ", act_zmp = " << hrp::Vector3(act_zmp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl; + hrp::Vector3 tmpnew_refzmp; + tmpnew_refzmp = foot_origin_rot.transpose()*(new_refzmp-foot_origin_pos); // Actual world -> foot origin relative + std::cerr << "[" << print_str << "] " + << "new_zmp = " << hrp::Vector3(tmpnew_refzmp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) + << ", dif_zmp = " << hrp::Vector3((tmpnew_refzmp-ref_zmp)*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl; + } + + std::vector ee_name; + // distribute new ZMP into foot force & moment + std::vector tmp_ref_force, tmp_ref_moment; + std::vector limb_gains; + std::vector ee_forcemoment_distribution_weight; + std::vector tmp_toeheel_ratio; + double tmp_total_fz = 0.0; + if (control_mode == MODE_ST) { + std::vector ee_pos, cop_pos; + std::vector ee_rot; + std::vector is_contact_list; + is_contact_list.reserve(stikp.size()); + for (size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + if (!is_ik_enable[i]) continue; + hrp::Link* target = m_robot->link(ikp.target_name); + rel_ee_rot_for_ik.push_back(foot_origin_rot.transpose() * (target->R * ikp.localR)); + ee_pos.push_back(target->p + target->R * ikp.localp); + cop_pos.push_back(target->p + target->R * ikp.localCOPPos); + ee_rot.push_back(target->R * ikp.localR); + limb_gains.push_back(ikp.swing_support_gain); + tmp_ref_force.push_back(hrp::Vector3(foot_origin_rot * ref_force[i])); + tmp_ref_moment.push_back(hrp::Vector3(foot_origin_rot * ref_moment[i])); + rel_ee_pos.push_back(foot_origin_rot.transpose() * (ee_pos.back() - foot_origin_pos)); + rel_ee_rot.push_back(foot_origin_rot.transpose() * ee_rot.back()); + if (!is_feedback_control_enable[i]) continue; + ee_name.push_back(ikp.ee_name); + rel_ee_name.push_back(ee_name.back()); + is_contact_list.push_back(act_contact_states[i]); + // std::cerr << ee_forcemoment_distribution_weight[i] << std::endl; + ee_forcemoment_distribution_weight.push_back(hrp::dvector6::Zero(6,1)); + for (size_t j = 0; j < 6; j++) { + ee_forcemoment_distribution_weight[i][j] = ikp.eefm_ee_forcemoment_distribution_weight[j]; + } + tmp_toeheel_ratio.push_back(toeheel_ratio[i]); + tmp_total_fz += tmp_ref_force.back()(2); + } + + // All state variables are foot_origin coords relative + if (DEBUGP) { + std::cerr << "[" << print_str << "] ee values" << std::endl; + hrp::Vector3 tmpp; + for (size_t i = 0; i < ee_name.size(); i++) { + tmpp = foot_origin_rot.transpose()*(ee_pos[i]-foot_origin_pos); + std::cerr << "[" << print_str << "] " + << "ee_pos (" << ee_name[i] << ") = " << hrp::Vector3(tmpp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")); + tmpp = foot_origin_rot.transpose()*(cop_pos[i]-foot_origin_pos); + std::cerr << ", cop_pos (" << ee_name[i] << ") = " << hrp::Vector3(tmpp*1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[mm]" << std::endl; + } + } + + // truncate ZMP + if (use_zmp_truncation && !is_walking) { + Eigen::Vector2d tmp_new_refzmp(new_refzmp.head(2)); + szd->get_vertices(support_polygon_vetices); + szd->calc_convex_hull(support_polygon_vetices, ref_contact_states, ee_pos, ee_rot); + if (!szd->is_inside_support_polygon(tmp_new_refzmp, hrp::Vector3::Zero(), true, std::string(print_str))) new_refzmp.head(2) = tmp_new_refzmp; + } + + // Distribute ZMP into each EE force/moment at each COP + if (on_ground) { + if (st_algorithm == OpenHRP::AutoBalancerService::EEFM) { + // Modified version of distribution in Equation (4)-(6) and (10)-(13) in the paper [1]. + szd->distributeZMPToForceMoments(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, + new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), + tmp_total_fz, dt, + DEBUGP, std::string(print_str)); + } else if (st_algorithm == OpenHRP::AutoBalancerService::EEFMQP) { + szd->distributeZMPToForceMomentsQP(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, + new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), + tmp_total_fz, dt, + DEBUGP, std::string(print_str), + (st_algorithm == OpenHRP::AutoBalancerService::EEFMQPCOP)); + } else if (st_algorithm == OpenHRP::AutoBalancerService::EEFMQPCOP) { + szd->distributeZMPToForceMomentsPseudoInverse(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, + new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), + tmp_total_fz, dt, + DEBUGP, std::string(print_str), + (st_algorithm == OpenHRP::AutoBalancerService::EEFMQPCOP), is_contact_list); + } else if (st_algorithm == OpenHRP::AutoBalancerService::EEFMQPCOP2) { + szd->distributeZMPToForceMomentsPseudoInverse2(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, + new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), + foot_origin_rot * ref_total_force, foot_origin_rot * ref_total_moment, + ee_forcemoment_distribution_weight, + tmp_total_fz, dt, + DEBUGP, std::string(print_str)); + } + } + // for debug output + new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos); + } + + // foor modif + if (control_mode == MODE_ST) { + hrp::Vector3 f_diff(hrp::Vector3::Zero()); + std::vector large_swing_f_diff(3, false); + // moment control + act_total_foot_origin_moment = hrp::Vector3::Zero(); + for (size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + std::vector large_swing_m_diff(3, false); + if (!is_feedback_control_enable[i]) continue; + hrp::Sensor* sensor = m_robot->sensor(ikp.sensor_name); + hrp::Link* target = m_robot->link(ikp.target_name); + // Convert moment at COP => moment at ee + size_t idx = contact_states_index_map[ikp.ee_name]; + ikp.ref_moment = tmp_ref_moment[idx] + ((target->R * ikp.localCOPPos + target->p) - (target->R * ikp.localp + target->p)).cross(tmp_ref_force[idx]); + ikp.ref_force = tmp_ref_force[idx]; + // Actual world frame => + hrp::Vector3 sensor_force = (sensor->link->R * sensor->localR) * hrp::Vector3(wrenches[i][0], wrenches[i][1], wrenches[i][2]); + hrp::Vector3 sensor_moment = (sensor->link->R * sensor->localR) * hrp::Vector3(wrenches[i][3], wrenches[i][4], wrenches[i][5]); + //hrp::Vector3 ee_moment = ((sensor->link->R * sensor->localPos + sensor->link->p) - (target->R * ikp.localCOPPos + target->p)).cross(sensor_force) + sensor_moment; + hrp::Vector3 ee_moment = ((sensor->link->R * sensor->localPos + sensor->link->p) - (target->R * ikp.localp + target->p)).cross(sensor_force) + sensor_moment; + // <= Actual world frame + hrp::Matrix33 ee_R(target->R * ikp.localR); + // Actual ee frame => + ikp.ref_moment = ee_R.transpose() * ikp.ref_moment; + ikp.ref_force = ee_R.transpose() * ikp.ref_force; + sensor_force = ee_R.transpose() * sensor_force; + ee_moment = ee_R.transpose() * ee_moment; + if ( i == 0 ) f_diff += -1*sensor_force; + else f_diff += sensor_force; + for (size_t j = 0; j < 3; ++j) { + if ((!ref_contact_states[i] || !act_contact_states[i]) && fabs(ikp.ref_force(j) - sensor_force(j)) > eefm_swing_damping_force_thre[j]) large_swing_f_diff[j] = true; + if ((!ref_contact_states[i] || !act_contact_states[i]) && (fabs(ikp.ref_moment(j) - ee_moment(j)) > eefm_swing_damping_moment_thre[j])) large_swing_m_diff[j] = true; + } + // Moment limitation + ikp.ref_moment = ee_R * vlimit((ee_R.transpose() * ikp.ref_moment), ikp.eefm_ee_moment_limit); + // calcDampingControl + // ee_d_foot_rpy and ee_d_foot_pos is (actual) end effector coords relative value because these use end effector coords relative force & moment + { // Rot + // Basically Equation (16) and (17) in the paper [1] + hrp::Vector3 tmp_damping_gain; + for (size_t j = 0; j < 3; ++j) { + double tmp_damping = ikp.eefm_rot_damping_gain(j) * (is_single_walking ? 1.0 : 4.0); + if (!eefm_use_swing_damping || !large_swing_m_diff[j]) tmp_damping_gain(j) = (1-transition_smooth_gain) * tmp_damping * 10 + transition_smooth_gain * tmp_damping; + else tmp_damping_gain(j) = (1-transition_smooth_gain) * eefm_swing_rot_damping_gain(j) * 10 + transition_smooth_gain * eefm_swing_rot_damping_gain(j); + } + ikp.ee_d_foot_rpy = calcDampingControl(ikp.ref_moment, ee_moment, ikp.ee_d_foot_rpy, tmp_damping_gain, ikp.eefm_rot_time_const); + ikp.ee_d_foot_rpy = vlimit(ikp.ee_d_foot_rpy, -1 * ikp.eefm_rot_compensation_limit, ikp.eefm_rot_compensation_limit); + } + if (!eefm_use_force_difference_control) { // Pos + hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * ikp.eefm_pos_damping_gain * 10 + transition_smooth_gain * ikp.eefm_pos_damping_gain; + ikp.ee_d_foot_pos = calcDampingControl(ikp.ref_force, sensor_force, ikp.ee_d_foot_pos, tmp_damping_gain, ikp.eefm_pos_time_const_support); + ikp.ee_d_foot_pos = vlimit(ikp.ee_d_foot_pos, -1 * ikp.eefm_pos_compensation_limit, ikp.eefm_pos_compensation_limit); + } + // Convert force & moment as foot origin coords relative + ikp.ref_moment = foot_origin_rot.transpose() * ee_R * ikp.ref_moment; + ikp.ref_force = foot_origin_rot.transpose() * ee_R * ikp.ref_force; + sensor_force = foot_origin_rot.transpose() * ee_R * sensor_force; + ee_moment = foot_origin_rot.transpose() * ee_R * ee_moment; + ikp.d_foot_rpy = foot_origin_rot.transpose() * ee_R * ikp.ee_d_foot_rpy; + ikp.d_foot_pos = foot_origin_rot.transpose() * ee_R * ikp.ee_d_foot_pos; + if ( joint_control_mode == OpenHRP::RobotHardwareService::TORQUE ) { + ikp.d_foot_rpy = hrp::Vector3::Zero(); + ikp.d_foot_pos = hrp::Vector3::Zero(); + } + // tilt Check : only flat plane is supported + { + hrp::Vector3 plane_x = target_ee_R[i].col(0); + hrp::Vector3 plane_y = target_ee_R[i].col(1); + hrp::Matrix33 act_ee_R_world = target->R * stikp[i].localR; + hrp::Vector3 normal_vector = act_ee_R_world.col(2); + /* projected_normal = c1 * plane_x + c2 * plane_y : c1 = plane_x.dot(normal_vector), c2 = plane_y.dot(normal_vector) because (normal-vector - projected_normal) is orthogonal to plane */ + projected_normal.at(i) = plane_x.dot(normal_vector) * plane_x + plane_y.dot(normal_vector) * plane_y; + act_force.at(i) = sensor_force; + } + //act_total_foot_origin_moment += (target->R * ikp.localCOPPos + target->p).cross(sensor_force) + ee_moment; + act_total_foot_origin_moment += (target->R * ikp.localp + target->p - foot_origin_pos).cross(sensor_force) + ee_moment; + } + act_total_foot_origin_moment = foot_origin_rot.transpose() * act_total_foot_origin_moment; + + if (eefm_use_force_difference_control) { + // fxyz control + // foot force difference control version + // Basically Equation (18) in the paper [1] + hrp::Vector3 ref_f_diff = (stikp[1].ref_force-stikp[0].ref_force); + if (ref_contact_states != prev_ref_contact_states) pos_ctrl = (foot_origin_rot.transpose() * prev_act_foot_origin_rot) * pos_ctrl; + if (eefm_use_swing_damping) { + hrp::Vector3 tmp_damping_gain; + for (size_t i = 0; i < 3; ++i) { + if (!large_swing_f_diff[i]) tmp_damping_gain(i) = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain(i) * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain(i); + else tmp_damping_gain(i) = (1-transition_smooth_gain) * eefm_swing_pos_damping_gain(i) * 10 + transition_smooth_gain * eefm_swing_pos_damping_gain(i); + } + pos_ctrl = calcDampingControl (ref_f_diff, f_diff, pos_ctrl, + tmp_damping_gain, stikp[0].eefm_pos_time_const_support); + } else { + hrp::Vector3 tmp_damping = stikp[0].eefm_pos_damping_gain * (is_single_walking ? 1.0 : 4.0); + if ( (ref_contact_states[contact_states_index_map["rleg"]] && ref_contact_states[contact_states_index_map["lleg"]]) // Reference : double support phase + || (act_contact_states[0] && act_contact_states[1]) ) { // Actual : double support phase + // Temporarily use first pos damping gain (stikp[0]) + hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * tmp_damping * 10 + transition_smooth_gain * tmp_damping; + pos_ctrl = calcDampingControl (ref_f_diff, f_diff, pos_ctrl, + tmp_damping_gain, stikp[0].eefm_pos_time_const_support); + } else { + double remain_swing_time; + if ( !ref_contact_states[contact_states_index_map["rleg"]] ) { // rleg swing + remain_swing_time = controlSwingSupportTime[contact_states_index_map["rleg"]]; + } else { // lleg swing + remain_swing_time = controlSwingSupportTime[contact_states_index_map["lleg"]]; + } + // std::cerr << "st " << remain_swing_time << " rleg " << contact_states[contact_states_index_map["rleg"]] << " lleg " << contact_states[contact_states_index_map["lleg"]] << std::endl; + double tmp_ratio = std::max(0.0, std::min(1.0, 1.0 - (remain_swing_time-eefm_pos_margin_time)/eefm_pos_transition_time)); // 0=>1 + // Temporarily use first pos damping gain (stikp[0]) + hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * tmp_damping * 10 + transition_smooth_gain * tmp_damping; + hrp::Vector3 tmp_time_const = (1-tmp_ratio)*eefm_pos_time_const_swing*hrp::Vector3::Ones()+tmp_ratio*stikp[0].eefm_pos_time_const_support; + pos_ctrl = calcDampingControl (tmp_ratio * ref_f_diff, tmp_ratio * f_diff, pos_ctrl, tmp_damping_gain, tmp_time_const); + } + } + // zctrl = vlimit(zctrl, -0.02, 0.02); + // Temporarily use first pos compensation limit (stikp[0]) + pos_ctrl = vlimit(pos_ctrl, -1 * stikp[0].eefm_pos_compensation_limit * 2, stikp[0].eefm_pos_compensation_limit * 2); + // Divide pos_ctrl into rfoot and lfoot + stikp[0].d_foot_pos = -0.5 * pos_ctrl; + stikp[1].d_foot_pos = 0.5 * pos_ctrl; + } + if (DEBUGP) { + std::cerr << "[" << print_str << "] Control values" << std::endl; + if (eefm_use_force_difference_control) { + std::cerr << "[" << print_str << "] " + << "pos_ctrl = [" << pos_ctrl(0)*1e3 << " " << pos_ctrl(1)*1e3 << " "<< pos_ctrl(2)*1e3 << "] [mm]" << std::endl; + } + for (size_t i = 0; i < ee_name.size(); i++) { + std::cerr << "[" << print_str << "] " + << "d_foot_pos (" << ee_name[i] << ") = [" << stikp[i].d_foot_pos(0)*1e3 << " " << stikp[i].d_foot_pos(1)*1e3 << " " << stikp[i].d_foot_pos(2)*1e3 << "] [mm], " + << "d_foot_rpy (" << ee_name[i] << ") = [" << stikp[i].d_foot_rpy(0)*180.0/M_PI << " " << stikp[i].d_foot_rpy(1)*180.0/M_PI << " " << stikp[i].d_foot_rpy(2)*180.0/M_PI << "] [deg]" << std::endl; + } + } + // foot force independent damping control + // for (size_t i = 0; i < 2; i++) { + // f_zctrl[i] = calcDampingControl (ref_force[i](2), + // fz[i], f_zctrl[i], eefm_pos_damping_gain, eefm_pos_time_const); + // f_zctrl[i] = vlimit(f_zctrl[i], -0.05, 0.05); + // } + calcDiffFootOriginExtMoment (); + } + } // st_algorithm == OpenHRP::AutoBalancerService::EEFM + + if ( joint_control_mode == OpenHRP::RobotHardwareService::TORQUE && control_mode == MODE_ST ) setSwingSupportJointServoGains(); + calcExternalForce(foot_origin_rot * act_cog + foot_origin_pos, foot_origin_rot * new_refzmp + foot_origin_pos, foot_origin_rot); // foot origin relative => Actual world frame + calcTorque(foot_origin_rot); + + for ( int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = qrefv[i]; + } + m_robot->rootLink()->p = target_root_p; + m_robot->rootLink()->R = target_root_R; + if ( !(control_mode == MODE_IDLE || control_mode == MODE_AIR) ) { + for (size_t i = 0; i < jpe_v.size(); i++) { + if (is_ik_enable[i]) { + for ( int j = 0; j < jpe_v[i]->numJoints(); j++ ){ + int idx = jpe_v[i]->joint(j)->jointId; + m_robot->joint(idx)->q = qorg[idx]; + } + } + } + m_robot->rootLink()->p(0) = current_root_p(0); + m_robot->rootLink()->p(1) = current_root_p(1); + m_robot->rootLink()->R = current_root_R; + m_robot->calcForwardKinematics(); + } + if (control_mode != MODE_ST) d_pos_z_root = 0.0; + prev_act_foot_origin_rot = foot_origin_rot; + prev_act_foot_origin_pos = foot_origin_pos; +} + +void Stabilizer::waitSTTransition() +{ + // Wait condition + // 1. Check transition_count : Wait until transition is finished + // 2. Check control_mode : Once control_mode is SYNC mode, wait until control_mode moves to the next mode (MODE_AIR or MODE_IDLE) + bool flag = (control_mode == MODE_SYNC_TO_AIR || control_mode == MODE_SYNC_TO_IDLE); + while (transition_count != 0 || + (flag ? !(control_mode == MODE_IDLE || control_mode == MODE_AIR) : false) ) { + usleep(10); + flag = (control_mode == MODE_SYNC_TO_AIR || control_mode == MODE_SYNC_TO_IDLE); + } + usleep(10); +} + +void Stabilizer::sync_2_st () +{ + std::cerr << "[" << print_str << "] [" + << "] Sync IDLE => ST" << std::endl; + d_rpy[0] = d_rpy[1] = 0; + pos_ctrl = hrp::Vector3::Zero(); + prev_ref_foot_origin_rot = hrp::Matrix33::Identity(); + is_after_walking = false; + if (!after_walking_interpolator->isEmpty()) after_walking_interpolator->clear(); + for (size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + ikp.target_ee_diff_p = hrp::Vector3::Zero(); + ikp.d_pos_swing = ikp.prev_d_pos_swing = hrp::Vector3::Zero(); + ikp.d_rpy_swing = ikp.prev_d_rpy_swing = hrp::Vector3::Zero(); + ikp.target_ee_diff_p_filter->reset(hrp::Vector3::Zero()); + ikp.d_foot_pos = ikp.ee_d_foot_pos = ikp.d_foot_rpy = ikp.ee_d_foot_rpy = hrp::Vector3::Zero(); + ikp.omega.angle() = 0.0; + swing_modification_interpolator[ikp.ee_name]->clear(); + is_foot_touch[i] = true; + } + if (on_ground) { + transition_count = -1 * calcMaxTransitionCount(); + control_mode = MODE_ST; + } else { + transition_count = 0; + control_mode = MODE_AIR; + } +} + +void Stabilizer::sync_2_idle () +{ + std::cerr << "[" << print_str << "] [" + << "] Sync ST => IDLE" << std::endl; + transition_count = calcMaxTransitionCount(); + for (int i = 0; i < m_robot->numJoints(); i++ ) { + transition_joint_q[i] = m_robot->joint(i)->q; + } +} + +void Stabilizer::stopSTEmergency() +{ + std::cerr << "[" << print_str << "] stop stabilizer mode for emergency" << std::endl; + for ( std::map::iterator it = swing_modification_interpolator.begin(); it != swing_modification_interpolator.end(); it++ ) { + it->second->clear(); + } + double tmp_ratio = 1.0; + transition_interpolator->clear(); + transition_interpolator->set(&tmp_ratio); + tmp_ratio = 0.0; + transition_interpolator->setGoal(&tmp_ratio, 0.8, true); + // for (int i = 0; i < m_robot->numJoints(); i++ ) { + // diff_q[i] = qorg[i] - qRef[i]; + // } + control_mode = MODE_IDLE; +} + +void Stabilizer::startStabilizer(void) +{ + waitSTTransition(); // Wait until all transition has finished + { + Guard guard(m_mutex); + if ( control_mode == MODE_IDLE ) { + std::cerr << "[" << print_str << "] " << "Start ST" << std::endl; + sync_2_st(); + } + } + waitSTTransition(); + if ( joint_control_mode == OpenHRP::RobotHardwareService::TORQUE ) { + std::cerr << "[" << print_str << "] " << "Moved to ST command pose and sync to TORQUE mode" << std::endl; + m_robotHardwareService0->setServoGainPercentage("all",100);//tmp + m_robotHardwareService0->setServoTorqueGainPercentage("all",100); + for(size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + hrp::JointPathExPtr jpe = jpe_v[i]; + for(size_t j = 0; j < ikp.support_pgain.size(); j++) { + m_robotHardwareService0->setServoPGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.support_pgain(j),3); + m_robotHardwareService0->setServoDGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.support_dgain(j),3); + } + } + is_reset_torque = false; + } + std::cerr << "[" << print_str << "] " << "Start ST DONE" << std::endl; +} + +void Stabilizer::stopStabilizer(void) +{ + waitSTTransition(); // Wait until all transition has finished + if ( joint_control_mode == OpenHRP::RobotHardwareService::TORQUE ) { + double tmp_time = 3.0; + m_robotHardwareService0->setServoPGainPercentageWithTime("all",100,tmp_time); + m_robotHardwareService0->setServoDGainPercentageWithTime("all",100,tmp_time); + usleep(tmp_time * 1e6); + m_robotHardwareService0->setServoTorqueGainPercentage("all",0); + is_reset_torque = true; + } + { + Guard guard(m_mutex); + if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) { + std::cerr << "[" << print_str << "] " << "Stop ST" << std::endl; + control_mode = (control_mode == MODE_ST) ? MODE_SYNC_TO_IDLE : MODE_IDLE; + } + } + waitSTTransition(); + std::cerr << "[" << print_str << "] " << "Stop ST DONE" << std::endl; +} + +// Damping control functions +// Basically Equation (14) in the paper [1] +double Stabilizer::calcDampingControl (const double tau_d, const double tau, const double prev_d, + const double DD, const double TT) +{ + return (1/DD * (tau_d - tau) - 1/TT * prev_d) * dt + prev_d; +}; + +// Retrieving only +hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT) +{ + return (- prev_d.cwiseQuotient(TT)) * dt + prev_d; +}; + +// Retrieving only +double Stabilizer::calcDampingControl (const double prev_d, const double TT) +{ + return - 1/TT * prev_d * dt + prev_d; +}; + +hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d, + const hrp::Vector3& DD, const hrp::Vector3& TT) +{ + return ((tau_d - tau).cwiseQuotient(DD) - prev_d.cwiseQuotient(TT)) * dt + prev_d; +}; + +void Stabilizer::calcContactMatrix (hrp::dmatrix& tm, const std::vector& contact_p) +{ + // tm.resize(6,6*contact_p.size()); + // tm.setZero(); + // for (size_t c = 0; c < contact_p.size(); c++) { + // for (size_t i = 0; i < 6; i++) tm(i,(c*6)+i) = 1.0; + // hrp::Matrix33 cm; + // rats::outer_product_matrix(cm, contact_p[c]); + // for (size_t i = 0; i < 3; i++) + // for (size_t j = 0; j < 3; j++) tm(i+3,(c*6)+j) = cm(i,j); + // } +} + +void Stabilizer::calcTorque (const hrp::Matrix33& rot) +{ + m_robot->calcForwardKinematics(true, true); + // buffers for the unit vector method + hrp::Vector3 root_w_x_v; + hrp::Vector3 g(0, 0, 9.80665); + root_w_x_v = m_robot->rootLink()->w.cross(m_robot->rootLink()->vo + m_robot->rootLink()->w.cross(m_robot->rootLink()->p)); + m_robot->rootLink()->dvo = g - root_w_x_v; // dv = g, dw = 0 + m_robot->rootLink()->dw.setZero(); + + hrp::Vector3 root_f; + hrp::Vector3 root_t; + m_robot->calcInverseDynamics(m_robot->rootLink(), root_f, root_t); + // if (loop % 200 == 0) { + // std::cerr << ":mass " << m_robot->totalMass() << std::endl; + // std::cerr << ":cog "; rats::print_vector(std::cerr, m_robot->calcCM()); + // for(int i = 0; i < m_robot->numJoints(); ++i){ + // std::cerr << "(list :" << m_robot->link(i)->name << " " + // << m_robot->joint(i)->jointId << " " + // << m_robot->link(i)->m << " "; + // hrp::Vector3 tmpc = m_robot->link(i)->p + m_robot->link(i)->R * m_robot->link(i)->c; + // rats::print_vector(std::cerr, tmpc, false); + // std::cerr << " "; + // rats::print_vector(std::cerr, m_robot->link(i)->c, false); + // std::cerr << ")" << std::endl; + // } + // } + // if (loop % 200 == 0) { + // std::cerr << ":IV1 (list "; + // for(int i = 0; i < m_robot->numJoints(); ++i){ + // std::cerr << "(list :" << m_robot->joint(i)->name << " " << m_robot->joint(i)->u << ")"; + // } + // std::cerr << ")" << std::endl; + // } + hrp::dmatrix contact_mat, contact_mat_inv; + std::vector contact_p; + for (size_t j = 0; j < 2; j++) contact_p.push_back(m_robot->sensor(stikp[j].sensor_name)->link->p); + calcContactMatrix(contact_mat, contact_p); + hrp::calcSRInverse(contact_mat, contact_mat_inv, 0.0); + hrp::dvector root_ft(6); + for (size_t j = 0; j < 3; j++) root_ft(j) = root_f(j); + for (size_t j = 0; j < 3; j++) root_ft(j+3) = root_t(j); + hrp::dvector contact_ft(2*6); + contact_ft = contact_mat_inv * root_ft; + // if (loop%200==0) { + // std::cerr << ":mass " << m_robot->totalMass() << std::endl; + // // std::cerr << ":ftv "; rats::print_vector(std::cerr, ftv); + // // std::cerr << ":aa "; rats::print_matrix(std::cerr, aa); + // // std::cerr << ":dv "; rats::print_vector(std::cerr, dv); + // } + // for (size_t j = 0; j < 2; j++) {//numContacts + // hrp::JointPathEx jm = hrp::JointPathEx(m_robot, m_robot->rootLink(), m_robot->sensor(stikp[j].sensor_name)->link, dt); + if ( control_mode == MODE_ST ) { + for (size_t j = 0; j < stikp.size(); j++) { + STIKParam& ikp = stikp[j]; + hrp::Link* target = m_robot->link(ikp.target_name); + size_t idx = contact_states_index_map[ikp.ee_name]; + const hrp::JointPath jm(m_robot->rootLink(), target); + hrp::dmatrix JJ(6, jm.numJoints()); + jm.calcJacobian(JJ); + hrp::dvector ft(6); + // for (size_t i = 0; i < 6; i++) ft(i) = contact_ft(i+j*6); + ft.segment(0,3) = rot * ikp.ref_force; + ft.segment(3,3) = rot * ikp.ref_moment; + ft.segment(3,3) += (target->R * ikp.localp).cross(rot * ikp.ref_force); + hrp::dvector tq_from_extft(jm.numJoints()); + tq_from_extft = JJ.transpose() * ft; + // if (loop%200==0) { + // std::cerr << ":ft "; rats::print_vector(std::cerr, ft); + // std::cerr << ":JJ "; rats::print_matrix(std::cerr, JJ); + // std::cerr << ":tq_from_extft "; rats::print_vector(std::cerr, tq_from_extft); + // } + for (size_t i = 0; i < jm.numJoints(); i++) jm.joint(i)->u -= tq_from_extft(i); + } + } + //hrp::dmatrix MM(6,m_robot->numJoints()); + //m_robot->calcMassMatrix(MM); + // if (loop % 200 == 0) { + // std::cerr << ":INVDYN2 (list "; rats::print_vector(std::cerr, root_f, false); + // std::cerr << " "; rats::print_vector(std::cerr, root_t, false); + // std::cerr << ")" << std::endl; + // // hrp::dvector tqv(m_robot->numJoints()); + // // for(int i = 0; i < m_robot->numJoints(); ++i){p + // // tqv[m_robot->joint(i)->jointId] = m_robot->joint(i)->u; + // // } + // // std::cerr << ":IV2 "; rats::print_vector(std::cerr, tqv); + // std::cerr << ":IV2 (list "; + // for(int i = 0; i < m_robot->numJoints(); ++i){ + // std::cerr << "(list :" << m_robot->joint(i)->name << " " << m_robot->joint(i)->u << ")"; + // } + // std::cerr << ")" << std::endl; + // } +}; + +void Stabilizer::setSwingSupportJointServoGains() +{ + static double tmp_landing2support_transition_time = landing2support_transition_time; + for (size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + hrp::JointPathExPtr jpe = jpe_v[i]; + if (ikp.contact_phase == SWING_PHASE && !ref_contact_states[i] && controlSwingSupportTime[i] < swing2landing_transition_time+landing_phase_time) { // SWING -> LANDING + ikp.contact_phase = LANDING_PHASE; + ikp.phase_time = 0; + for(size_t j = 0; j < ikp.support_pgain.size(); j++) { + m_robotHardwareService0->setServoPGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.landing_pgain(j),swing2landing_transition_time); + m_robotHardwareService0->setServoDGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.landing_dgain(j),swing2landing_transition_time); + } + } + if (ikp.contact_phase == LANDING_PHASE && act_contact_states[i] && ref_contact_states[i] && ikp.phase_time > swing2landing_transition_time) { // LANDING -> SUPPORT + ikp.contact_phase = SUPPORT_PHASE; + ikp.phase_time = 0; + tmp_landing2support_transition_time = std::min(landing2support_transition_time, controlSwingSupportTime[i]); + for(size_t j = 0; j < ikp.support_pgain.size(); j++) { + m_robotHardwareService0->setServoPGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.support_pgain(j),tmp_landing2support_transition_time); + m_robotHardwareService0->setServoDGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.support_dgain(j),tmp_landing2support_transition_time); + } + } + // if (ikp.contact_phase == SUPPORT_PHASE && !act_contact_states[i] && ikp.phase_time > tmp_landing2support_transition_time) { // SUPPORT -> SWING + if (ikp.contact_phase == SUPPORT_PHASE && !act_contact_states[i] && ikp.phase_time > tmp_landing2support_transition_time+support_phase_min_time + && ( (ref_contact_states[i] && controlSwingSupportTime[i] < 0.2) || !ref_contact_states[i] )) { // SUPPORT -> SWING + ikp.contact_phase = SWING_PHASE; + ikp.phase_time = 0; + for(size_t j = 0; j < ikp.support_pgain.size(); j++) { + m_robotHardwareService0->setServoPGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.swing_pgain(j),support2swing_transition_time); + m_robotHardwareService0->setServoDGainPercentageWithTime(jpe->joint(j)->name.c_str(),ikp.swing_dgain(j),support2swing_transition_time); + } + } + ikp.phase_time += dt; + } +} + +void Stabilizer::calcExternalForce(const hrp::Vector3& cog, const hrp::Vector3& zmp, const hrp::Matrix33& rot) +{ + // cog, zmp must be in the same coords with stikp.ref_forece + hrp::Vector3 total_force = hrp::Vector3::Zero(); + for (size_t j = 0; j < stikp.size(); j++) { + total_force(2) += stikp[j].ref_force(2); // only fz + } + total_force.segment(0,2) = (cog.segment(0,2) - zmp.segment(0,2))*total_force(2)/(cog(2) - zmp(2)); // overwrite fxy + for (size_t j = 0; j < stikp.size(); j++) { + STIKParam& ikp = stikp[j]; + if (on_ground && total_force(2) > 1e-6) ikp.ref_force.segment(0,2) += rot.transpose() * total_force.segment(0,2) * ikp.ref_force(2)/total_force(2);// set fx,fy + } +} + +void Stabilizer::moveBasePosRotForBodyRPYControl () +{ + // Body rpy control + // Basically Equation (1) and (2) in the paper [1] + hrp::Vector3 ref_root_rpy = hrp::rpyFromRot(target_root_R); + bool is_root_rot_limit = false; + for (size_t i = 0; i < 2; i++) { + d_rpy[i] = transition_smooth_gain * (eefm_body_attitude_control_gain[i] * (ref_root_rpy(i) - act_base_rpy(i)) - 1/eefm_body_attitude_control_time_const[i] * d_rpy[i]) * dt + d_rpy[i]; + d_rpy[i] = vlimit(d_rpy[i], -1 * root_rot_compensation_limit[i], root_rot_compensation_limit[i]); + is_root_rot_limit = is_root_rot_limit || (std::fabs(std::fabs(d_rpy[i]) - root_rot_compensation_limit[i] ) < 1e-5); // near the limit + } + rats::rotm3times(current_root_R, target_root_R, hrp::rotFromRpy(d_rpy[0], d_rpy[1], 0)); + m_robot->rootLink()->R = current_root_R; + m_robot->rootLink()->p = target_root_p + target_root_R * rel_cog - current_root_R * rel_cog; + m_robot->calcForwardKinematics(); + current_base_rpy = hrp::rpyFromRot(m_robot->rootLink()->R); + current_base_pos = m_robot->rootLink()->p; + if ( DEBUGP || (is_root_rot_limit && loop%200==0) ) { + std::cerr << "[" << print_str << "] Root rot control" << std::endl; + if (is_root_rot_limit) std::cerr << "[" << print_str << "] Root rot limit reached!!" << std::endl; + std::cerr << "[" << print_str << "] ref = [" << rad2deg(ref_root_rpy(0)) << " " << rad2deg(ref_root_rpy(1)) << "], " + << "act = [" << rad2deg(act_base_rpy(0)) << " " << rad2deg(act_base_rpy(1)) << "], " + << "cur = [" << rad2deg(current_base_rpy(0)) << " " << rad2deg(current_base_rpy(1)) << "], " + << "limit = [" << rad2deg(root_rot_compensation_limit[0]) << " " << rad2deg(root_rot_compensation_limit[1]) << "][deg]" << std::endl; + } +}; + +double Stabilizer::vlimit(double value, double llimit_value, double ulimit_value) +{ + if (value > ulimit_value) { + return ulimit_value; + } else if (value < llimit_value) { + return llimit_value; + } + return value; +} + +hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, double llimit_value, double ulimit_value) +{ + hrp::Vector3 ret; + for (size_t i = 0; i < 3; i++) { + if (value(i) > ulimit_value) { + ret(i) = ulimit_value; + } else if (value(i) < llimit_value) { + ret(i) = llimit_value; + } else { + ret(i) = value(i); + } + } + return ret; +} + +hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, const hrp::Vector3& limit_value) +{ + hrp::Vector3 ret; + for (size_t i = 0; i < 3; i++) { + if (value(i) > limit_value(i)) { + ret(i) = limit_value(i); + } else if (value(i) < -1 * limit_value(i)) { + ret(i) = -1 * limit_value(i); + } else { + ret(i) = value(i); + } + } + return ret; +} + +hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, const hrp::Vector3& llimit_value, const hrp::Vector3& ulimit_value) +{ + hrp::Vector3 ret; + for (size_t i = 0; i < 3; i++) { + if (value(i) > ulimit_value(i)) { + ret(i) = ulimit_value(i); + } else if (value(i) < llimit_value(i)) { + ret(i) = llimit_value(i); + } else { + ret(i) = value(i); + } + } + return ret; +} + +void Stabilizer::calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot) +{ + rats::coordinates leg_c[2], tmpc; + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 ex = hrp::Vector3::UnitX(); + for (size_t i = 0; i < stikp.size(); i++) { + if (stikp[i].ee_name.find("leg") == std::string::npos) continue; + hrp::Link* target = m_robot->sensor(stikp[i].sensor_name)->link; + leg_c[i].pos = target->p + target->R * foot_origin_offset[i]; + hrp::Vector3 xv1(target->R * ex); + xv1(2)=0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + leg_c[i].rot(0,0) = xv1(0); leg_c[i].rot(1,0) = xv1(1); leg_c[i].rot(2,0) = xv1(2); + leg_c[i].rot(0,1) = yv1(0); leg_c[i].rot(1,1) = yv1(1); leg_c[i].rot(2,1) = yv1(2); + leg_c[i].rot(0,2) = ez(0); leg_c[i].rot(1,2) = ez(1); leg_c[i].rot(2,2) = ez(2); + } + if (ref_contact_states[contact_states_index_map["rleg"]] && + ref_contact_states[contact_states_index_map["lleg"]]) { + rats::mid_coords(tmpc, 0.5, leg_c[0], leg_c[1]); + foot_origin_pos = tmpc.pos; + foot_origin_rot = tmpc.rot; + } else if (ref_contact_states[contact_states_index_map["rleg"]]) { + foot_origin_pos = leg_c[contact_states_index_map["rleg"]].pos; + foot_origin_rot = leg_c[contact_states_index_map["rleg"]].rot; + } else { + foot_origin_pos = leg_c[contact_states_index_map["lleg"]].pos; + foot_origin_rot = leg_c[contact_states_index_map["lleg"]].rot; + } +} + +bool Stabilizer::calcZMP(hrp::Vector3& ret_zmp, const double zmp_z, hrp::Matrix33 foot_origin_rot) +{ + double tmpzmpx = 0; + double tmpzmpy = 0; + double tmpfz = 0, tmpfz2 = 0.0; + for (size_t i = 0; i < stikp.size(); i++) { + if (!is_zmp_calc_enable[i]) continue; + hrp::ForceSensor* sensor = m_robot->sensor(stikp[i].sensor_name); + hrp::Vector3 fsp = sensor->link->p + sensor->link->R * sensor->localPos; + hrp::Matrix33 tmpR; + rats::rotm3times(tmpR, sensor->link->R, sensor->localR); + hrp::Vector3 nf = tmpR * hrp::Vector3(wrenches[i][0], wrenches[i][1], wrenches[i][2]); + hrp::Vector3 nm = tmpR * hrp::Vector3(wrenches[i][3], wrenches[i][4], wrenches[i][5]); + hrp::Vector3 rel_nf = foot_origin_rot.transpose() * nf; // foot origin frame + tmpzmpx += nf(2) * fsp(0) - (fsp(2) - zmp_z) * nf(0) - nm(1); + tmpzmpy += nf(2) * fsp(1) - (fsp(2) - zmp_z) * nf(1) + nm(0); + tmpfz += nf(2); + // calc ee-local COP + hrp::Link* target = m_robot->link(stikp[i].target_name); + hrp::Matrix33 eeR = target->R * stikp[i].localR; + hrp::Vector3 ee_fsp = eeR.transpose() * (fsp - (target->p + target->R * stikp[i].localp)); // ee-local force sensor pos + nf = eeR.transpose() * nf; + nm = eeR.transpose() * nm; + // ee-local total moment and total force at ee position + double tmpcopmy = nf(2) * ee_fsp(0) - nf(0) * ee_fsp(2) - nm(1); + double tmpcopmx = nf(2) * ee_fsp(1) - nf(1) * ee_fsp(2) + nm(0); + double tmpcopfz = nf(2); + copInfo[i*3] = tmpcopmx; + copInfo[i*3+1] = tmpcopmy; + copInfo[i*3+2] = tmpcopfz; + prev_act_force[i] = 0.95 * prev_act_force[i] + 0.05 * rel_nf; // filter, cut off 5[Hz] + tmpfz2 += prev_act_force[i](2); + } + if (tmpfz2 < contact_decision_threshold) { + ret_zmp = act_zmp; + return false; // in the air + } else { + ret_zmp = hrp::Vector3(tmpzmpx / tmpfz, tmpzmpy / tmpfz, zmp_z); + return true; // on ground + } +}; + +void Stabilizer::calcStateForEmergencySignal() +{ + // COP Check + bool is_cop_outside = false; + if (DEBUGP) { + std::cerr << "[" << print_str << "] Check Emergency State (seq = " << (is_seq_interpolating?"interpolating":"empty") << ")" << std::endl; + } + if (on_ground && transition_count == 0 && control_mode == MODE_ST) { + if (DEBUGP) { + std::cerr << "[" << print_str << "] COP check" << std::endl; + } + for (size_t i = 0; i < stikp.size(); i++) { + if (stikp[i].ee_name.find("leg") == std::string::npos) continue; + // check COP inside + if (copInfo[i*3+2] > 20.0 ) { + hrp::Vector3 tmpcop(copInfo[i*3+1]/copInfo[i*3+2], copInfo[i*3]/copInfo[i*3+2], 0); + is_cop_outside = is_cop_outside || + (!szd->is_inside_foot(tmpcop, stikp[i].ee_name=="lleg", cop_check_margin) || + szd->is_front_of_foot(tmpcop, cop_check_margin) || + szd->is_rear_of_foot(tmpcop, cop_check_margin)); + if (DEBUGP) { + std::cerr << "[" << print_str << "] [" << stikp[i].ee_name << "] " + << "outside(" << !szd->is_inside_foot(tmpcop, stikp[i].ee_name=="lleg", cop_check_margin) << ") " + << "front(" << szd->is_front_of_foot(tmpcop, cop_check_margin) << ") " + << "rear(" << szd->is_rear_of_foot(tmpcop, cop_check_margin) << ")" << std::endl; + } + } else { + is_cop_outside = true; + } + } + } else { + is_cop_outside = false; + } + // CP Check + bool is_cp_outside = false; + is_emergency_motion = false; + if (on_ground && transition_count == 0 && control_mode == MODE_ST) { + Eigen::Vector2d tmp_cp = act_cp.head(2); + std::vector tmp_cs(2,true); + hrp::Vector3 ref_root_rpy = hrp::rpyFromRot(target_root_R); + // stop manipulation + if (!is_walking) { + szd->get_margined_vertices(margined_support_polygon_vetices); + szd->calc_convex_hull(margined_support_polygon_vetices, act_contact_states, rel_ee_pos, rel_ee_rot); + is_cp_outside = !szd->is_inside_support_polygon(tmp_cp, - sbp_cog_offset); + } + // start emregency stepping + szd->get_vertices(support_polygon_vetices); + szd->calc_convex_hull(support_polygon_vetices, act_contact_states, rel_ee_pos, rel_ee_rot); + is_emergency_step = !szd->is_inside_support_polygon(tmp_cp, - sbp_cog_offset); + // // comment in if you use emergency motion + // if (!is_walking && falling_direction != 0) { + // if (((falling_direction == 1 || falling_direction == 2) && std::fabs(rad2deg(ref_root_rpy(1))) > 0.5) || + // ((falling_direction == 3 || falling_direction == 4) && std::fabs(rad2deg(ref_root_rpy(0))) > 0.5)) + // is_emergency_motion = true; + // } + if (DEBUGP) { + std::cerr << "[" << print_str << "] CP value " << "[" << act_cp(0) << "," << act_cp(1) << "] [m], " + << "sbp cog offset [" << sbp_cog_offset(0) << " " << sbp_cog_offset(1) << "], outside ? " + << (is_cp_outside?"Outside":"Inside") + << std::endl; + } + if (is_cp_outside) { + if (initial_cp_too_large_error || loop % static_cast (0.2/dt) == 0 ) { // once per 0.2[s] + std::cerr << "[" << print_str << "] [" + << "] CP too large error " << "[" << act_cp(0) << "," << act_cp(1) << "] [m]" << std::endl; + } + initial_cp_too_large_error = false; + } else { + initial_cp_too_large_error = true; + } + } + // tilt Check // cannot use + hrp::Vector3 fall_direction = hrp::Vector3::Zero(); + bool is_falling = false, will_fall = false; + // { + // double total_force = 0.0; + // for (size_t i = 0; i < stikp.size(); i++) { + // if (is_zmp_calc_enable[i]) { + // if (is_walking) { + // if (projected_normal.at(i).norm() > sin(tilt_margin[0])) { + // will_fall = true; + // if (m_will_fall_counter[i] % static_cast (1.0/dt) == 0 ) { // once per 1.0[s] + // std::cerr << "[" << print_str << "] [" + // << "] " << stikp[i].ee_name << " cannot support total weight, " + // << "swgsuptime : " << controlSwingSupportTime[i] << ", state : " << ref_contact_states[i] + // << ", otherwise robot will fall down toward " << "(" << projected_normal.at(i)(0) << "," << projected_normal.at(i)(1) << ") direction" << std::endl; + // } + // m_will_fall_counter[i]++; + // } else { + // m_will_fall_counter[i] = 0; + // } + // } + // fall_direction += projected_normal.at(i) * act_force.at(i).norm(); + // total_force += act_force.at(i).norm(); + // } + // } + // if (on_ground && transition_count == 0 && control_mode == MODE_ST) { + // fall_direction = fall_direction / total_force; + // } else { + // fall_direction = hrp::Vector3::Zero(); + // } + // if (fall_direction.norm() > sin(tilt_margin[1])) { + // is_falling = true; + // if (m_is_falling_counter % static_cast (0.2/dt) == 0) { // once per 0.2[s] + // std::cerr << "[" << print_str << "] [" + // << "] robot is falling down toward " << "(" << fall_direction(0) << "," << fall_direction(1) << ") direction" << std::endl; + // } + // m_is_falling_counter++; + // } else { + // m_is_falling_counter = 0; + // } + // } + // Total check for emergency signal + switch (emergency_check_mode) { + case OpenHRP::AutoBalancerService::NO_CHECK: + is_emergency = false; + break; + case OpenHRP::AutoBalancerService::COP: + is_emergency = is_cop_outside && is_seq_interpolating; + break; + case OpenHRP::AutoBalancerService::CP: + is_emergency = is_cp_outside; + break; + case OpenHRP::AutoBalancerService::TILT: + is_emergency = will_fall || is_falling; + break; + default: + break; + } + if (DEBUGP) { + std::cerr << "[" << print_str << "] EmergencyCheck (" + << (emergency_check_mode == OpenHRP::AutoBalancerService::NO_CHECK?"NO_CHECK": (emergency_check_mode == OpenHRP::AutoBalancerService::COP?"COP":"CP") ) + << ") " << (is_emergency?"emergency":"non-emergency") << std::endl; + } + rel_ee_pos.clear(); + rel_ee_rot.clear(); + rel_ee_name.clear(); + rel_ee_rot_for_ik.clear(); +}; + +void Stabilizer::calcSwingSupportLimbGain () +{ + for (size_t i = 0; i < stikp.size(); i++) { + STIKParam& ikp = stikp[i]; + if (ref_contact_states[i]) { // Support + // Limit too large support time increment. Max time is 3600.0[s] = 1[h], this assumes that robot's one step time is smaller than 1[h]. + ikp.support_time = std::min(3600.0, ikp.support_time+dt); + // In some PC, does not work because the first line is optimized out. + // ikp.support_time += dt; + // ikp.support_time = std::min(3600.0, ikp.support_time); + if (ikp.support_time > eefm_pos_transition_time) { + ikp.swing_support_gain = (controlSwingSupportTime[i] / eefm_pos_transition_time); + } else { + ikp.swing_support_gain = (ikp.support_time / eefm_pos_transition_time); + } + ikp.swing_support_gain = std::max(0.0, std::min(1.0, ikp.swing_support_gain)); + } else { // Swing + ikp.swing_support_gain = 0.0; + ikp.support_time = 0.0; + } + } + if (DEBUGP) { + std::cerr << "[" << print_str << "] SwingSupportLimbGain = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << stikp[i].swing_support_gain << " "; + std::cerr << "], ref_contact_states = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << ref_contact_states[i] << " "; + std::cerr << "], sstime = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << controlSwingSupportTime[i] << " "; + std::cerr << "], toeheel_ratio = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << toeheel_ratio[i] << " "; + std::cerr << "], support_time = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << stikp[i].support_time << " "; + std::cerr << "]" << std::endl; + } +} + +void Stabilizer::calcTPCC() { + // stabilizer loop + // Choi's feedback law + hrp::Vector3 cog = m_robot->calcCM(); + hrp::Vector3 newcog = hrp::Vector3::Zero(); + hrp::Vector3 dcog(ref_cog - act_cog); + hrp::Vector3 dzmp(ref_zmp - act_zmp); + for (size_t i = 0; i < 2; i++) { + double uu = ref_cogvel(i) - k_tpcc_p[i] * transition_smooth_gain * dzmp(i) + + k_tpcc_x[i] * transition_smooth_gain * dcog(i); + newcog(i) = uu * dt + cog(i); + } + + moveBasePosRotForBodyRPYControl (); + + // target at ee => target at link-origin + hrp::Vector3 target_link_p[stikp.size()]; + hrp::Matrix33 target_link_R[stikp.size()]; + for (size_t i = 0; i < stikp.size(); i++) { + rats::rotm3times(target_link_R[i], target_ee_R[i], stikp[i].localR.transpose()); + target_link_p[i] = target_ee_p[i] - target_ee_R[i] * stikp[i].localCOPPos; + } + // solveIK + // IK target is link origin pos and rot, not ee pos and rot. + //for (size_t jj = 0; jj < 5; jj++) { + size_t max_ik_loop_count = 0; + for (size_t i = 0; i < stikp.size(); i++) { + if (max_ik_loop_count < stikp[i].ik_loop_count) max_ik_loop_count = stikp[i].ik_loop_count; + } + for (size_t jj = 0; jj < max_ik_loop_count; jj++) { + hrp::Vector3 tmpcm = m_robot->calcCM(); + for (size_t i = 0; i < 2; i++) { + m_robot->rootLink()->p(i) = m_robot->rootLink()->p(i) + 0.9 * (newcog(i) - tmpcm(i)); + } + m_robot->calcForwardKinematics(); + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + jpe_v[i]->calcInverseKinematics2Loop(target_link_p[i], target_link_R[i], 1.0, stikp[i].avoid_gain, stikp[i].reference_gain, &qrefv, transition_smooth_gain); + } + } + } +} + +void Stabilizer::calcEEForceMomentControl() +{ + // stabilizer loop + // return to referencea + m_robot->rootLink()->R = target_root_R; + m_robot->rootLink()->p = target_root_p; + for ( int i = 0; i < m_robot->numJoints(); i++ ) { + m_robot->joint(i)->q = qrefv[i]; + } + for (size_t i = 0; i < jpe_v.size(); i++) { + if (is_ik_enable[i]) { + for ( int j = 0; j < jpe_v[i]->numJoints(); j++ ){ + int idx = jpe_v[i]->joint(j)->jointId; + m_robot->joint(idx)->q = qorg[idx]; + } + } + } + // Fix for toe joint + for (size_t i = 0; i < jpe_v.size(); i++) { + if (is_ik_enable[i]) { + if (jpe_v[i]->numJoints() == 7) { + int idx = jpe_v[i]->joint(jpe_v[i]->numJoints() -1)->jointId; + m_robot->joint(idx)->q = qrefv[idx]; + } + } + } + + // State calculation for swing ee compensation + // joint angle : current control output + // root pos : target root p + // root rot : actual root rot + { + // Calc status + m_robot->rootLink()->p = target_root_p; + m_robot->rootLink()->R = act_root_R; + m_robot->calcForwardKinematics(); + hrp::Vector3 foot_origin_pos; + hrp::Matrix33 foot_origin_rot; + calcFootOriginCoords (foot_origin_pos, foot_origin_rot); + // Calculate foot_origin_coords-relative ee pos and rot + // Subtract them from target_ee_diff_xx + for (size_t i = 0; i < stikp.size(); i++) { + hrp::Link* target = m_robot->link(stikp[i].target_name); + stikp[i].target_ee_diff_p -= foot_origin_rot.transpose() * (target->p + target->R * stikp[i].localp - foot_origin_pos); + stikp[i].act_theta = Eigen::AngleAxisd(foot_origin_rot.transpose() * target->R * stikp[i].localR); + } + } + + // State calculation for control : calculate "current" state + // joint angle : current control output + // root pos : target + keep COG against rpy control + // root rot : target + rpy control + moveBasePosRotForBodyRPYControl (); + + // Convert d_foot_pos in foot origin frame => "current" world frame + hrp::Vector3 foot_origin_pos; + hrp::Matrix33 foot_origin_rot; + calcFootOriginCoords (foot_origin_pos, foot_origin_rot); + + // Swing ee compensation. + if (use_act_states && use_force_sensor) calcSwingEEModification(); + + // solveIK + // IK target is link origin pos and rot, not ee pos and rot. + std::vector tmpp(stikp.size()); + std::vector tmpR(stikp.size()); + double tmp_d_pos_z_root = 0.0; + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + // Add damping_control compensation to target value + if (is_feedback_control_enable[i]) { + rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-1*stikp[i].ee_d_foot_rpy)); + // foot force difference control version + // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl; + // foot force independent damping control + tmpp[i] = target_ee_p[i] - (foot_origin_rot * stikp[i].d_foot_pos); + } else { + tmpp[i] = target_ee_p[i]; + tmpR[i] = target_ee_R[i]; + } + // Add swing ee compensation + rats::rotm3times(tmpR[i], tmpR[i], hrp::rotFromRpy(rel_ee_rot_for_ik[i].transpose() * stikp[i].d_rpy_swing)); + tmpp[i] = tmpp[i] + (foot_origin_rot * stikp[i].d_pos_swing); + } + } + + limbStretchAvoidanceControl(tmpp ,tmpR); + + // IK + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + for (size_t jj = 0; jj < stikp[i].ik_loop_count; jj++) { + jpe_v[i]->calcInverseKinematics2Loop(tmpp[i], tmpR[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain, + //stikp[i].localCOPPos; + stikp[i].localp, + stikp[i].localR); + } + } + } + prev_ref_foot_origin_rot = foot_origin_rot; +} + +// Swing ee compensation. +// Calculate compensation values to minimize the difference between "current" foot-origin-coords-relative pos and rot and "target" foot-origin-coords-relative pos and rot for swing ee. +// Input : target_ee_diff_p, ref_theta, act_theta +// Output : d_pos_swing, d_rpy_swing +void Stabilizer::calcSwingEEModification () +{ + for (size_t i = 0; i < stikp.size(); i++) { + // Calc compensation values + double limit_pos = 50 * 1e-3; // 50[mm] limit + double limit_rot = deg2rad(30); // 30[deg] limit + if (ref_contact_states != prev_ref_contact_states) { + stikp[i].d_pos_swing = (ref_foot_origin_rot.transpose() * prev_ref_foot_origin_rot) * stikp[i].d_pos_swing; + stikp[i].d_rpy_swing = (ref_foot_origin_rot.transpose() * prev_ref_foot_origin_rot) * stikp[i].d_rpy_swing; + stikp[i].prev_d_pos_swing = (ref_foot_origin_rot.transpose() * prev_ref_foot_origin_rot) * stikp[i].prev_d_pos_swing; + stikp[i].prev_d_rpy_swing = (ref_foot_origin_rot.transpose() * prev_ref_foot_origin_rot) * stikp[i].prev_d_rpy_swing; + } + if (ref_contact_states[contact_states_index_map[stikp[i].ee_name]] || act_contact_states[contact_states_index_map[stikp[i].ee_name]]) { + // If actual contact or target contact is ON, do not use swing ee compensation. Exponential zero retrieving. + if (!is_foot_touch[i] && is_walking) { + double tmp_ratio = 1.0; + swing_modification_interpolator[stikp[i].ee_name]->clear(); + swing_modification_interpolator[stikp[i].ee_name]->set(&tmp_ratio); + tmp_ratio = 0.0; + swing_modification_interpolator[stikp[i].ee_name]->setGoal(&tmp_ratio, stikp[i].touchoff_remain_time, true); + is_foot_touch[i] = true; + } + } else if (swing_modification_interpolator[stikp[i].ee_name]->isEmpty()) { + /* position */ + { + hrp::Vector3 tmpdiffp = stikp[i].eefm_swing_pos_spring_gain.cwiseProduct(stikp[i].target_ee_diff_p) * dt + stikp[i].d_pos_swing; + double lvlimit = -50 * 1e-3 * dt, uvlimit = 50 * 1e-3 * dt; // 50 [mm/s] + hrp::Vector3 limit_by_lvlimit = stikp[i].prev_d_pos_swing + lvlimit * hrp::Vector3::Ones(); + hrp::Vector3 limit_by_uvlimit = stikp[i].prev_d_pos_swing + uvlimit * hrp::Vector3::Ones(); + stikp[i].d_pos_swing = vlimit(vlimit(tmpdiffp, -1 * limit_pos, limit_pos), limit_by_lvlimit, limit_by_uvlimit); + } + /* rotation */ + { + Eigen::AngleAxisd prev_omega = stikp[i].omega; + stikp[i].omega = (stikp[i].act_theta.inverse() * stikp[i].ref_theta); + stikp[i].omega.angle() *= stikp[i].eefm_swing_rot_spring_gain(0) * dt; + stikp[i].omega = stikp[i].omega * prev_omega; + hrp::Vector3 tmpdiffr = hrp::rpyFromRot(stikp[i].omega.toRotationMatrix()); + double lvlimit = deg2rad(-40.0*dt), uvlimit = deg2rad(40.0*dt); // 20 [deg/s] + hrp::Vector3 limit_by_lvlimit = stikp[i].prev_d_rpy_swing + lvlimit * hrp::Vector3::Ones(); + hrp::Vector3 limit_by_uvlimit = stikp[i].prev_d_rpy_swing + uvlimit * hrp::Vector3::Ones(); + stikp[i].d_rpy_swing = vlimit(vlimit(tmpdiffr, -1 * limit_rot, limit_rot), limit_by_lvlimit, limit_by_uvlimit); + } + is_foot_touch[i] = false; + touchdown_d_pos[i] = stikp[i].d_pos_swing; + touchdown_d_rpy[i] = stikp[i].d_rpy_swing; + } + if (!swing_modification_interpolator[stikp[i].ee_name]->isEmpty()) { + double tmp_ratio = 0.0; + swing_modification_interpolator[stikp[i].ee_name]->setGoal(&tmp_ratio, stikp[i].touchoff_remain_time, true); + swing_modification_interpolator[stikp[i].ee_name]->get(&tmp_ratio, true); + stikp[i].d_pos_swing = touchdown_d_pos[i] * tmp_ratio; + stikp[i].d_rpy_swing = touchdown_d_rpy[i] * tmp_ratio; + } + stikp[i].prev_d_pos_swing = stikp[i].d_pos_swing; + stikp[i].prev_d_rpy_swing = stikp[i].d_rpy_swing; + } + if (DEBUGP) { + std::cerr << "[" << print_str << "] Swing foot control" << std::endl; + for (size_t i = 0; i < stikp.size(); i++) { + std::cerr << "[" << print_str << "] " + << "d_rpy_swing (" << stikp[i].ee_name << ") = " << (stikp[i].d_rpy_swing / M_PI * 180.0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[deg], " + << "d_pos_swing (" << stikp[i].ee_name << ") = " << (stikp[i].d_pos_swing * 1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl; + } + } +}; + +void Stabilizer::limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R) +{ + double tmp_d_pos_z_root = 0.0, prev_d_pos_z_root = d_pos_z_root; + if (use_limb_stretch_avoidance) { + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); + hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) + double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1); + if (targetp.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp)); + } + } + } + // Change root link height depending on limb length + d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root; + } else { + d_pos_z_root = calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const); + } + d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + limb_stretch_avoidance_vlimit[0], prev_d_pos_z_root + limb_stretch_avoidance_vlimit[1]); + m_robot->rootLink()->p(2) += d_pos_z_root; +} + +void Stabilizer::setBoolSequenceParam (std::vector& st_bool_values, const OpenHRP::AutoBalancerService::BoolSequence& output_bool_values, const std::string& prop_name) +{ + std::vector prev_values; + prev_values.resize(st_bool_values.size()); + copy (st_bool_values.begin(), st_bool_values.end(), prev_values.begin()); + if (st_bool_values.size() != output_bool_values.length()) { + std::cerr << "[" << print_str << "] " << prop_name << " cannot be set. Length " << st_bool_values.size() << " != " << output_bool_values.length() << std::endl; + } else if ( (control_mode != MODE_IDLE) ) { + std::cerr << "[" << print_str << "] " << prop_name << " cannot be set. Current control_mode is " << control_mode << std::endl; + } else { + for (size_t i = 0; i < st_bool_values.size(); i++) { + st_bool_values[i] = output_bool_values[i]; + } + } + std::cerr << "[" << print_str << "] " << prop_name << " is "; + for (size_t i = 0; i < st_bool_values.size(); i++) { + std::cerr <<"[" << st_bool_values[i] << "]"; + } + std::cerr << "(set = "; + for (size_t i = 0; i < output_bool_values.length(); i++) { + std::cerr <<"[" << output_bool_values[i] << "]"; + } + std::cerr << ", prev = "; + for (size_t i = 0; i < prev_values.size(); i++) { + std::cerr <<"[" << prev_values[i] << "]"; + } + std::cerr << ")" << std::endl; +}; + +void Stabilizer::setBoolSequenceParamWithCheckContact (std::vector& st_bool_values, const OpenHRP::AutoBalancerService::BoolSequence& output_bool_values, const std::string& prop_name) +{ + std::vector prev_values; + prev_values.resize(st_bool_values.size()); + copy (st_bool_values.begin(), st_bool_values.end(), prev_values.begin()); + if (st_bool_values.size() != output_bool_values.length()) { + std::cerr << "[" << print_str << "] " << prop_name << " cannot be set. Length " << st_bool_values.size() << " != " << output_bool_values.length() << std::endl; + } else if ( control_mode == MODE_IDLE ) { + for (size_t i = 0; i < st_bool_values.size(); i++) { + st_bool_values[i] = output_bool_values[i]; + } + } else { + std::vector failed_indices; + for (size_t i = 0; i < st_bool_values.size(); i++) { + if ( (st_bool_values[i] != output_bool_values[i]) ) { // If mode change + if (!ref_contact_states[i] ) { // reference contact_states should be OFF + st_bool_values[i] = output_bool_values[i]; + } else { + failed_indices.push_back(i); + } + } + } + if (failed_indices.size() > 0) { + std::cerr << "[" << print_str << "] " << prop_name << " cannot be set partially. failed_indices is ["; + for (size_t i = 0; i < failed_indices.size(); i++) { + std::cerr << failed_indices[i] << " "; + } + std::cerr << "]" << std::endl; + } + } + std::cerr << "[" << print_str << "] " << prop_name << " is "; + for (size_t i = 0; i < st_bool_values.size(); i++) { + std::cerr <<"[" << st_bool_values[i] << "]"; + } + std::cerr << "(set = "; + for (size_t i = 0; i < output_bool_values.length(); i++) { + std::cerr <<"[" << output_bool_values[i] << "]"; + } + std::cerr << ", prev = "; + for (size_t i = 0; i < prev_values.size(); i++) { + std::cerr <<"[" << prev_values[i] << "]"; + } + std::cerr << ")" << std::endl; +}; + +std::string Stabilizer::getStabilizerAlgorithmString (OpenHRP::AutoBalancerService::STAlgorithm _st_algorithm) +{ + switch (_st_algorithm) { + case OpenHRP::AutoBalancerService::TPCC: + return "TPCC"; + case OpenHRP::AutoBalancerService::EEFM: + return "EEFM"; + case OpenHRP::AutoBalancerService::EEFMQP: + return "EEFMQP"; + case OpenHRP::AutoBalancerService::EEFMQPCOP: + return "EEFMQPCOP"; + case OpenHRP::AutoBalancerService::EEFMQPCOP2: + return "EEFMQPCOP2"; + default: + return ""; + } +}; + +void Stabilizer::calcDiffFootOriginExtMoment () +{ + // calc reference ext moment around foot origin pos + // static const double grav = 9.80665; /* [m/s^2] */ + double mg = total_mass * eefm_gravitational_acceleration; + hrp::Vector3 ref_ext_moment = hrp::Vector3(mg * ref_cog(1) - ref_total_foot_origin_moment(0), + -mg * ref_cog(0) - ref_total_foot_origin_moment(1), + 0); + // calc act ext moment around foot origin pos + hrp::Vector3 act_ext_moment = hrp::Vector3(mg * act_cog(1) - act_total_foot_origin_moment(0), + -mg * act_cog(0) - act_total_foot_origin_moment(1), + 0); + // Do not calculate actual value if in the air, because of invalid act_zmp. + if ( !on_ground ) act_ext_moment = ref_ext_moment; + // Calc diff + diff_foot_origin_ext_moment = ref_ext_moment - act_ext_moment; + if (DEBUGP) { + std::cerr << "[" << print_str << "] DiffStaticBalancePointOffset" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_ext_moment = " << ref_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm], " + << "act_ext_moment = " << act_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm], " + << "diff ext_moment = " << diff_foot_origin_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl; + } +}; diff --git a/rtc/AutoBalancer/Stabilizer.h b/rtc/AutoBalancer/Stabilizer.h new file mode 100644 index 00000000000..863aa08ca09 --- /dev/null +++ b/rtc/AutoBalancer/Stabilizer.h @@ -0,0 +1,217 @@ +// -*- C++ -*- +/*! + * @file Stabilizer.h + * @brief stabilizer component + * @date $Date$ + * + * $Id$ + */ + +#ifndef STABILIZER_COMPONENT_H +#define STABILIZER_COMPONENT_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Service implementation headers +// +#include "AutoBalancerService_impl.h" +#include "ZMPDistributor.h" +#include "../ImpedanceController/JointPathEx.h" +#include "../ImpedanceController/RatsMatrix.h" +#include "../TorqueFilter/IIRFilter.h" +#include "../SequencePlayer/interpolator.h" +#include "hrpsys/idl/RobotHardwareService.hh" + +// + +// Service Consumer stub headers +// + +// + +/** + \brief sample RT component which has one data input port and one data output port + */ + +class Stabilizer +{ +private: + // Robot model + hrp::BodyPtr m_robot; + double dt; + std::string print_str; + +public: + enum cphase {LANDING_PHASE=-1, SWING_PHASE=0, SUPPORT_PHASE=1}; + struct STIKParam { + std::string target_name; // Name of end link + std::string ee_name; // Name of ee (e.g., rleg, lleg, ...) + std::string sensor_name; // Name of force sensor in the limb + std::string parent_name; // Name of parent ling in the limb + hrp::Vector3 localp; // Position of ee in end link frame (^{l}p_e = R_l^T (p_e - p_l)) + hrp::Vector3 localCOPPos; // Position offset of reference COP in end link frame (^{l}p_{cop} = R_l^T (p_{cop} - p_l) - ^{l}p_e) + hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e) + // For eefm + hrp::Vector3 d_foot_pos, ee_d_foot_pos, d_foot_rpy, ee_d_foot_rpy; + hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const, eefm_swing_rot_spring_gain, eefm_swing_pos_spring_gain, eefm_swing_rot_time_const, eefm_swing_pos_time_const, eefm_ee_moment_limit; + double eefm_pos_compensation_limit, eefm_rot_compensation_limit; + hrp::Vector3 ref_force, ref_moment; + hrp::dvector6 eefm_ee_forcemoment_distribution_weight; + double swing_support_gain, support_time; + // For swing ee modification + boost::shared_ptr > target_ee_diff_p_filter; + hrp::Vector3 target_ee_diff_p, d_pos_swing, d_rpy_swing, prev_d_pos_swing, prev_d_rpy_swing; + double touchoff_remain_time; + // IK parameter + double avoid_gain, reference_gain, max_limb_length, limb_length_margin; + size_t ik_loop_count; + Eigen::AngleAxisd ref_theta, act_theta, omega; + // joint servo control parameter + cphase contact_phase; + double phase_time; + hrp::dvector support_pgain,support_dgain,landing_pgain,landing_dgain, swing_pgain, swing_dgain; + }; + enum cmode {MODE_IDLE, MODE_AIR, MODE_ST, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_AIR} control_mode; + // members + std::map m_vfs; + std::vector jpe_v; + coil::Mutex m_mutex; + unsigned int m_debugLevel; + hrp::dvector transition_joint_q, qorg, qrefv; + std::vector stikp; + std::map contact_states_index_map; + std::vector ref_contact_states, prev_ref_contact_states, act_contact_states, prev_act_contact_states, is_ik_enable, is_feedback_control_enable, is_zmp_calc_enable; + std::vector toeheel_ratio; + int transition_count, loop; + int m_is_falling_counter; + std::vector m_will_fall_counter; + int is_air_counter, detection_count_to_air; + bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_limb_stretch_avoidance, use_zmp_truncation; + bool is_walking, is_estop_while_walking, is_emergency_step, is_single_walking, is_emergency_motion; + hrp::Vector3 current_root_p, target_root_p, ref_foot_origin_pos, prev_act_foot_origin_pos; + hrp::Matrix33 current_root_R, target_root_R, act_root_R, prev_act_foot_origin_rot, prev_ref_foot_origin_rot, target_foot_origin_rot, ref_foot_origin_rot; + std::vector target_ee_p, rel_ee_pos, act_ee_p, projected_normal, act_force, ref_force, ref_moment; + std::vector target_ee_R, rel_ee_rot, act_ee_R, rel_ee_rot_for_ik; + std::vector rel_ee_name; + rats::coordinates target_foot_midcoords; + hrp::Vector3 ref_zmp, ref_cog, ref_cp, ref_cogvel, rel_ref_cp, prev_ref_cog, prev_ref_zmp; + hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset, cp_offset, diff_cp, act_cmp; + hrp::Vector3 foot_origin_offset[2]; + std::vector prev_act_force; + double zmp_origin_off, transition_smooth_gain, d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], root_rot_compensation_limit[2]; + boost::shared_ptr > act_cogvel_filter; + OpenHRP::AutoBalancerService::STAlgorithm st_algorithm; + SimpleZMPDistributor* szd; + std::vector > support_polygon_vetices, margined_support_polygon_vetices; + // TPCC + double k_tpcc_p[2], k_tpcc_x[2], d_rpy[2], k_brot_p[2], k_brot_tc[2]; + // EEFM ST + double eefm_k1[2], eefm_k2[2], eefm_k3[2], eefm_zmp_delay_time_const[2], eefm_body_attitude_control_gain[2], eefm_body_attitude_control_time_const[2]; + double eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_gravitational_acceleration; + std::vector eefm_swing_damping_force_thre, eefm_swing_damping_moment_thre; + hrp::Vector3 new_refzmp, after_walking_refzmp, rel_cog, ref_zmp_aux, diff_foot_origin_ext_moment; + hrp::Vector3 pos_ctrl; + hrp::Vector3 ref_total_force, ref_total_moment; + // Total foot moment around the foot origin coords (relative to foot origin coords) + hrp::Vector3 ref_total_foot_origin_moment, act_total_foot_origin_moment; + hrp::Vector3 eefm_swing_pos_damping_gain, eefm_swing_rot_damping_gain; + double swing2landing_transition_time, landing_phase_time, landing2support_transition_time, support_phase_min_time, support2swing_transition_time; + double total_mass, transition_time, cop_check_margin, contact_decision_threshold; + std::vector cp_check_margin, tilt_margin; + OpenHRP::AutoBalancerService::EmergencyCheckMode emergency_check_mode; + hrp::dvector qRef, qCurrent, qRefSeq, controlSwingSupportTime, copInfo; + hrp::Matrix33 baseRot; + hrp::Vector3 zmpRef, basePos, rpy, baseRpy; + std::vector wrenches, ref_wrenches; + std::vector limbCOPOffset; + hrp::Vector3 foot_origin_pos; + hrp::Matrix33 foot_origin_rot; + bool use_act_states; + std::vector diff_q; + interpolator *transition_interpolator; + size_t falling_direction; + std::map swing_modification_interpolator; + std::vector is_foot_touch; + std::vector touchdown_d_pos, touchdown_d_rpy; + bool use_force_sensor; + // joint servo control + OpenHRP::RobotHardwareService::JointControlMode joint_control_mode; + RTC::CorbaConsumer m_robotHardwareService0; + bool is_reset_torque, is_after_walking; + interpolator *after_walking_interpolator; + bool use_footguided_stabilizer; + double footguided_balance_time_const; + double total_external_force_z; + size_t jump_time_count; + double jump_initial_velocity; + + Stabilizer(hrp::BodyPtr& _robot, const std::string& _print_str, const double& _dt) + : m_robot(_robot), print_str(_print_str), dt(_dt), + control_mode(MODE_IDLE), + st_algorithm(OpenHRP::AutoBalancerService::TPCC), + emergency_check_mode(OpenHRP::AutoBalancerService::NO_CHECK), + szd(NULL), joint_control_mode(OpenHRP::RobotHardwareService::POSITION), + m_debugLevel(0) + { + }; + ~Stabilizer() {}; + + void initStabilizer(const RTC::Properties& prop, const size_t& _num); + void execStabilizer(); + void getCurrentParameters (); + void getTargetParameters (); + void getActualParameters (); + void getActualParametersForST (); + void waitSTTransition(); + void sync_2_st (); + void sync_2_idle(); + void stopSTEmergency(); + inline int calcMaxTransitionCount () + { + return (transition_time / dt); + }; + void startStabilizer(void); + void stopStabilizer(void); + double calcDampingControl (const double tau_d, const double tau, const double prev_d, + const double DD, const double TT); + hrp::Vector3 calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT); + double calcDampingControl (const double prev_d, const double TT); + hrp::Vector3 calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d, + const hrp::Vector3& DD, const hrp::Vector3& TT); + void calcContactMatrix (hrp::dmatrix& tm, const std::vector& contact_p); + void setSwingSupportJointServoGains(); + void calcExternalForce (const hrp::Vector3& cog, const hrp::Vector3& zmp, const hrp::Matrix33& rot); + void calcTorque (const hrp::Matrix33& rot); + void moveBasePosRotForBodyRPYControl (); + double vlimit(double value, double llimit_value, double ulimit_value); + hrp::Vector3 vlimit(const hrp::Vector3& value, double llimit_value, double ulimit_value); + hrp::Vector3 vlimit(const hrp::Vector3& value, const hrp::Vector3& limit_value); + hrp::Vector3 vlimit(const hrp::Vector3& value, const hrp::Vector3& llimit_value, const hrp::Vector3& ulimit_value); + void calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot); + bool calcZMP(hrp::Vector3& ret_zmp, const double zmp_z, hrp::Matrix33 foot_origin_rot); + void calcStateForEmergencySignal(); + void calcSwingSupportLimbGain(); + void calcTPCC(); + void calcEEForceMomentControl(); + void calcSwingEEModification (); + void limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R); + void setBoolSequenceParam (std::vector& st_bool_values, const OpenHRP::AutoBalancerService::BoolSequence& output_bool_values, const std::string& prop_name); + void setBoolSequenceParamWithCheckContact (std::vector& st_bool_values, const OpenHRP::AutoBalancerService::BoolSequence& output_bool_values, const std::string& prop_name); + std::string getStabilizerAlgorithmString (OpenHRP::AutoBalancerService::STAlgorithm _st_algorithm); + inline bool isContact (const size_t idx) // 0 = right, 1 = left + { + return (prev_act_force[idx](2) > 25.0); + }; + void calcDiffFootOriginExtMoment (); +}; + +#endif // STABILIZER_COMPONENT_H diff --git a/rtc/AutoBalancer/ZMPDistributor.h b/rtc/AutoBalancer/ZMPDistributor.h new file mode 100644 index 00000000000..96aea02a6fc --- /dev/null +++ b/rtc/AutoBalancer/ZMPDistributor.h @@ -0,0 +1,1151 @@ +// -*- C++ -*- +/*! + * @file ZMPDistributor.h + * @brief ZMP distribution + * @date $Date$ + * + * $Id$ + */ + +#ifndef ZMP_DISTRIBUTOR_H +#define ZMP_DISTRIBUTOR_H + +#include +#include +#include +#include "../ImpedanceController/JointPathEx.h" +#include "../TorqueFilter/IIRFilter.h" +#include + +#ifdef USE_QPOASES +#include +using namespace qpOASES; +#endif + +class FootSupportPolygon +{ + std::vector > foot_vertices; // RLEG, LLEG +public: + FootSupportPolygon () {}; + bool inside_foot (size_t idx) + { + return true; + }; + Eigen::Vector2d get_foot_vertex (const size_t foot_idx, const size_t vtx_idx) + { + return foot_vertices[foot_idx][vtx_idx]; + }; + void set_vertices (const std::vector >& vs) { foot_vertices = vs; }; + void get_vertices (std::vector >& vs) { vs = foot_vertices; }; + void print_vertices (const std::string& str) + { + std::cerr << "[" << str << "] "; + for (size_t i = 0; i < foot_vertices.size(); i++) { + std::cerr << "vs = "; + for (size_t j = 0; j < foot_vertices[i].size(); j++) { + std::cerr << "[" << foot_vertices[i][j](0) << " " << foot_vertices[i][j](1) << "] "; + } + std::cerr << ((i==foot_vertices.size()-1)?"[m]":"[m], "); + } + std::cerr << std::endl;; + } +}; + +// + +class SimpleZMPDistributor +{ + FootSupportPolygon fs, fs_mgn; + double leg_inside_margin, leg_outside_margin, leg_front_margin, leg_rear_margin, wrench_alpha_blending; + boost::shared_ptr > alpha_filter; + std::vector convex_hull; + enum projected_point_region {LEFT, MIDDLE, RIGHT}; +public: + enum leg_type {RLEG, LLEG, RARM, LARM, BOTH, ALL}; + SimpleZMPDistributor (const double _dt) : wrench_alpha_blending (0.5) + { + alpha_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(1e7, _dt, 0.5)); // [Hz], Almost no filter by default + }; + + inline bool is_inside_foot (const hrp::Vector3& leg_pos, const bool is_lleg, const double margin = 0.0) + { + if (is_lleg) return (leg_pos(1) >= (-1 * leg_inside_margin + margin)) && (leg_pos(1) <= (leg_outside_margin - margin)); + else return (leg_pos(1) <= (leg_inside_margin - margin)) && (leg_pos(1) >= (-1 * leg_outside_margin + margin)); + }; + inline bool is_front_of_foot (const hrp::Vector3& leg_pos, const double margin = 0.0) + { + return leg_pos(0) >= (leg_front_margin - margin); + }; + inline bool is_rear_of_foot (const hrp::Vector3& leg_pos, const double margin = 0.0) + { + return leg_pos(0) <= (-1 * leg_rear_margin + margin); + }; + inline bool is_inside_support_polygon (Eigen::Vector2d& p, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false, const std::string& str = "") + { + // set any inner point(ip) and binary search two vertices(convex_hull[v_a], convex_hull[v_b]) between which p is. + p -= offset.head(2); + size_t n_ch = convex_hull.size(); + Eigen::Vector2d ip = (convex_hull[0] + convex_hull[n_ch/3] + convex_hull[2*n_ch/3]) / 3.0; + size_t v_a = 0, v_b = n_ch; + while (v_a + 1 < v_b) { + size_t v_c = (v_a + v_b) / 2; + if (calcCrossProduct(convex_hull[v_a], convex_hull[v_c], ip) > 0) { + if (calcCrossProduct(convex_hull[v_a], p, ip) > 0 && calcCrossProduct(convex_hull[v_c], p, ip) < 0) v_b = v_c; + else v_a = v_c; + } else { + if (calcCrossProduct(convex_hull[v_a], p, ip) < 0 && calcCrossProduct(convex_hull[v_c], p, ip) > 0) v_a = v_c; + else v_b = v_c; + } + } + v_b %= n_ch; + if (calcCrossProduct(convex_hull[v_a], convex_hull[v_b], p) >= 0) { + p += offset.head(2); + return true; + } else { + if (truncate_p) { + if (!calc_closest_boundary_point(p, v_a, v_b)) std::cerr << "[" << str << "] Cannot calculate closest boundary point on the convex hull" << std::endl; + } + p += offset.head(2); + return false; + } + }; + void print_params (const std::string& str) + { + std::cerr << "[" << str << "] leg_inside_margin = " << leg_inside_margin << "[m], leg_outside_margin = " << leg_outside_margin << "[m], leg_front_margin = " << leg_front_margin << "[m], leg_rear_margin = " << leg_rear_margin << "[m]" << std::endl; + std::cerr << "[" << str << "] wrench_alpha_blending = " << wrench_alpha_blending << ", alpha_cutoff_freq = " << alpha_filter->getCutOffFreq() << "[Hz]" << std::endl; + } + void print_vertices (const std::string& str) + { + fs.print_vertices(str); + }; + // Compare Vector2d for sorting lexicographically + static bool compare_eigen2d(const Eigen::Vector2d& lv, const Eigen::Vector2d& rv) + { + return lv(0) < rv(0) || (lv(0) == rv(0) && lv(1) < rv(1)); + }; + // Calculate 2D convex hull based on Andrew's algorithm + // Assume that the order of vs, ee, and cs is the same + void calc_convex_hull (const std::vector >& vs, const std::vector& cs, const std::vector& ee_pos, const std::vector & ee_rot) + { + // transform coordinate + std::vector tvs; + hrp::Vector3 tpos; + tvs.reserve(cs.size()*vs[0].size()); + for (size_t i = 0; i < cs.size(); i++) { + if (cs[i]) { + for (size_t j = 0; j < vs[i].size(); j++) { + tpos = ee_pos[i] + ee_rot[i] * hrp::Vector3(vs[i][j](0), vs[i][j](1), 0.0); + tvs.push_back(tpos.head(2)); + } + } + } + // calculate convex hull + int n_tvs = tvs.size(), n_ch = 0; + convex_hull.resize(2*n_tvs); + std::sort(tvs.begin(), tvs.end(), compare_eigen2d); + for (int i = 0; i < n_tvs; convex_hull[n_ch++] = tvs[i++]) + while (n_ch >= 2 && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--; + for (int i = n_tvs-2, j = n_ch+1; i >= 0; convex_hull[n_ch++] = tvs[i--]) + while (n_ch >= j && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--; + convex_hull.resize(n_ch-1); + }; + // Calculate closest boundary point on the convex hull + bool calc_closest_boundary_point (Eigen::Vector2d& p, size_t& right_idx, size_t& left_idx) { + size_t n_ch = convex_hull.size(); + Eigen::Vector2d cur_closest_point; + for (size_t i = 0; i < n_ch; i++) { + switch(calcProjectedPoint(cur_closest_point, p, convex_hull[left_idx], convex_hull[right_idx])) { + case MIDDLE: + p = cur_closest_point; + return true; + case LEFT: + right_idx = left_idx; + left_idx = (left_idx + 1) % n_ch; + if ((p - convex_hull[right_idx]).dot(convex_hull[left_idx] - convex_hull[right_idx]) <= 0) { + p = cur_closest_point; + return true; + } + case RIGHT: + left_idx = right_idx; + right_idx = (right_idx - 1) % n_ch; + if ((p - convex_hull[left_idx]).dot(convex_hull[right_idx] - convex_hull[left_idx]) <= 0) { + p = cur_closest_point; + return true; + } + } + } + return false; + } + // setter + void set_wrench_alpha_blending (const double a) { wrench_alpha_blending = a; }; + void set_leg_front_margin (const double a) { leg_front_margin = a; }; + void set_leg_rear_margin (const double a) { leg_rear_margin = a; }; + void set_leg_inside_margin (const double a) { leg_inside_margin = a; }; + void set_leg_outside_margin (const double a) { leg_outside_margin = a; }; + void set_alpha_cutoff_freq (const double a) { alpha_filter->setCutOffFreq(a); }; + void set_vertices (const std::vector >& vs) + { + fs.set_vertices(vs); + // leg_inside_margin = fs.get_foot_vertex(0, 0)(1); + // leg_front_margin = fs.get_foot_vertex(0, 0)(0); + // leg_rear_margin = std::fabs(fs.get_foot_vertex(0, 3)(0)); + }; + void set_vertices_from_margin_params () + { + std::vector > vec; + // RLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin)); + tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin)); + tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin)); + tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin)); + vec.push_back(tvec); + } + // LLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_outside_margin)); + tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_inside_margin)); + tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_inside_margin)); + tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_outside_margin)); + vec.push_back(tvec); + } + // { + // std::vector tvec; + // tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin)); + // tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin)); + // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin)); + // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin)); + // vec.push_back(tvec); + // } + // { + // std::vector tvec; + // tvec.push_back(Eigen::Vector2d(leg_front_margin, leg_inside_margin)); + // tvec.push_back(Eigen::Vector2d(leg_front_margin, -1*leg_outside_margin)); + // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, -1*leg_outside_margin)); + // tvec.push_back(Eigen::Vector2d(-1*leg_rear_margin, leg_inside_margin)); + // vec.push_back(tvec); + // } + set_vertices(vec); + }; + // Set vertices only for cp_check_margin for now + void set_vertices_from_margin_params (const std::vector& margin) + { + std::vector > vec; + // RLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[2])); + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[3]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[3]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[2])); + vec.push_back(tvec); + } + // LLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[3])); + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[2]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[2]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[3])); + vec.push_back(tvec); + } + fs_mgn.set_vertices(vec); + }; + // getter + double get_wrench_alpha_blending () { return wrench_alpha_blending; }; + double get_leg_front_margin () { return leg_front_margin; }; + double get_leg_rear_margin () { return leg_rear_margin; }; + double get_leg_inside_margin () { return leg_inside_margin; }; + double get_leg_outside_margin () { return leg_outside_margin; }; + double get_alpha_cutoff_freq () { return alpha_filter->getCutOffFreq(); }; + void get_vertices (std::vector >& vs) { fs.get_vertices(vs); }; + void get_margined_vertices (std::vector >& vs) { fs_mgn.get_vertices(vs); }; + // + double calcAlpha (const hrp::Vector3& tmprefzmp, + const std::vector& ee_pos, + const std::vector& ee_rot, + const std::vector& ee_name) + { + double alpha; + size_t l_idx, r_idx; + for (size_t i = 0; i < ee_name.size(); i++) { + if (ee_name[i]=="rleg") r_idx = i; + else l_idx = i; + } + hrp::Vector3 l_local_zmp = ee_rot[l_idx].transpose() * (tmprefzmp-ee_pos[l_idx]); + hrp::Vector3 r_local_zmp = ee_rot[r_idx].transpose() * (tmprefzmp-ee_pos[r_idx]); + + // std::cerr << "a " << l_local_zmp(0) << " " << l_local_zmp(1) << std::endl; + // std::cerr << "b " << r_local_zmp(0) << " " << r_local_zmp(1) << std::endl; + if ( is_inside_foot(l_local_zmp, true) && !is_front_of_foot(l_local_zmp) && !is_rear_of_foot(l_local_zmp)) { // new_refzmp is inside lfoot + alpha = 0.0; + } else if ( is_inside_foot(r_local_zmp, false) && !is_front_of_foot(r_local_zmp) && !is_rear_of_foot(r_local_zmp)) { // new_refzmp is inside rfoot + alpha = 1.0; + } else { + hrp::Vector3 ledge_foot; + hrp::Vector3 redge_foot; + // lleg + if (is_inside_foot(l_local_zmp, true) && is_front_of_foot(l_local_zmp)) { + ledge_foot = hrp::Vector3(leg_front_margin, l_local_zmp(1), 0.0); + } else if (!is_inside_foot(l_local_zmp, true) && is_front_of_foot(l_local_zmp)) { + ledge_foot = hrp::Vector3(leg_front_margin, -1 * leg_inside_margin, 0.0); + } else if (!is_inside_foot(l_local_zmp, true) && !is_front_of_foot(l_local_zmp) && !is_rear_of_foot(l_local_zmp)) { + ledge_foot = hrp::Vector3(l_local_zmp(0), -1 * leg_inside_margin, 0.0); + } else if (!is_inside_foot(l_local_zmp, true) && is_rear_of_foot(l_local_zmp)) { + ledge_foot = hrp::Vector3(-1 * leg_rear_margin, -1 * leg_inside_margin, 0.0); + } else { + ledge_foot = hrp::Vector3(-1 * leg_rear_margin, l_local_zmp(1), 0.0); + } + ledge_foot = ee_rot[l_idx] * ledge_foot + ee_pos[l_idx]; + // rleg + if (is_inside_foot(r_local_zmp, false) && is_front_of_foot(r_local_zmp)) { + redge_foot = hrp::Vector3(leg_front_margin, r_local_zmp(1), 0.0); + } else if (!is_inside_foot(r_local_zmp, false) && is_front_of_foot(r_local_zmp)) { + redge_foot = hrp::Vector3(leg_front_margin, leg_inside_margin, 0.0); + } else if (!is_inside_foot(r_local_zmp, false) && !is_front_of_foot(r_local_zmp) && !is_rear_of_foot(r_local_zmp)) { + redge_foot = hrp::Vector3(r_local_zmp(0), leg_inside_margin, 0.0); + } else if (!is_inside_foot(r_local_zmp, false) && is_rear_of_foot(r_local_zmp)) { + redge_foot = hrp::Vector3(-1 * leg_rear_margin, leg_inside_margin, 0.0); + } else { + redge_foot = hrp::Vector3(-1 * leg_rear_margin, r_local_zmp(1), 0.0); + } + redge_foot = ee_rot[r_idx] * redge_foot + ee_pos[r_idx]; + // calc alpha + hrp::Vector3 difp = redge_foot - ledge_foot; + alpha = difp.dot(tmprefzmp-ledge_foot)/difp.squaredNorm(); + } + // limit + if (alpha>1.0) alpha = 1.0; + if (alpha<0.0) alpha = 0.0; + return alpha; + }; + + double calcAlphaFromCOP (const hrp::Vector3& tmprefzmp, + const std::vector& cop_pos, + const std::vector& ee_name) + { + size_t l_idx, r_idx; + for (size_t i = 0; i < ee_name.size(); i++) { + if (ee_name[i]=="rleg") r_idx = i; + else l_idx = i; + } + hrp::Vector3 difp = (cop_pos[r_idx]-cop_pos[l_idx]); + double alpha = difp.dot(tmprefzmp - cop_pos[l_idx])/difp.dot(difp); + + // limit + if (alpha>1.0) alpha = 1.0; + if (alpha<0.0) alpha = 0.0; + return alpha; + }; + + void calcAlphaVector (std::vector& alpha_vector, + std::vector& fz_alpha_vector, + const std::vector& ee_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp) + { + double fz_alpha = calcAlpha(ref_zmp, ee_pos, ee_rot, ee_name); + double tmpalpha = calcAlpha(new_refzmp, ee_pos, ee_rot, ee_name), alpha; + // LPF + alpha = alpha_filter->passFilter(tmpalpha); + // Blending + fz_alpha = wrench_alpha_blending * fz_alpha + (1-wrench_alpha_blending) * alpha; + for (size_t i = 0; i < ee_name.size(); i++) { + alpha_vector[i]= (ee_name[i]=="rleg") ? alpha : 1-alpha; + fz_alpha_vector[i]=(ee_name[i]=="rleg") ? fz_alpha : 1-fz_alpha; + } + }; + + void calcAlphaVectorFromCOP (std::vector& alpha_vector, + std::vector& fz_alpha_vector, + const std::vector& cop_pos, + const std::vector& ee_name, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp) + { + double fz_alpha = calcAlphaFromCOP(ref_zmp, cop_pos, ee_name); + double tmpalpha = calcAlphaFromCOP(new_refzmp, cop_pos, ee_name), alpha; + // LPF + alpha = alpha_filter->passFilter(tmpalpha); + // Blending + fz_alpha = wrench_alpha_blending * fz_alpha + (1-wrench_alpha_blending) * alpha; + for (size_t i = 0; i < ee_name.size(); i++) { + alpha_vector[i]= (ee_name[i]=="rleg") ? alpha : 1-alpha; + fz_alpha_vector[i]=(ee_name[i]=="rleg") ? fz_alpha : 1-fz_alpha; + } + }; + + void calcAlphaVectorFromCOPDistanceCommon (std::vector& alpha_vector, + const std::vector& cop_pos, + const std::vector& ee_name, + const hrp::Vector3& ref_zmp) + { + std::vector distances; + double tmpdistance = 0; + for (size_t i = 0; i < ee_name.size(); i++) { + distances.push_back((cop_pos[i]-ref_zmp).norm()); + tmpdistance += (cop_pos[i]-ref_zmp).norm(); + } + for (size_t i = 0; i < ee_name.size(); i++) { + alpha_vector[i] = tmpdistance/distances[i]; + } + }; + + void calcAlphaVectorFromCOPDistance (std::vector& alpha_vector, + std::vector& fz_alpha_vector, + const std::vector& cop_pos, + const std::vector& ee_name, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp) + { + calcAlphaVectorFromCOPDistanceCommon(alpha_vector, cop_pos, ee_name, new_refzmp); + calcAlphaVectorFromCOPDistanceCommon(fz_alpha_vector, cop_pos, ee_name, ref_zmp); + for (size_t i = 0; i < ee_name.size(); i++) { + fz_alpha_vector[i] = wrench_alpha_blending * fz_alpha_vector[i] + (1-wrench_alpha_blending) * alpha_vector[i]; + } + }; + + void distributeZMPToForceMoments (std::vector& ref_foot_force, std::vector& ref_foot_moment, + const std::vector& ee_pos, + const std::vector& cop_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const std::vector& limb_gains, + const std::vector& toeheel_ratio, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, + const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "") + { + std::vector alpha_vector(2), fz_alpha_vector(2); + calcAlphaVector(alpha_vector, fz_alpha_vector, ee_pos, ee_rot, ee_name, new_refzmp, ref_zmp); + ref_foot_force[0] = hrp::Vector3(0,0, fz_alpha_vector[0] * total_fz); + ref_foot_force[1] = hrp::Vector3(0,0, fz_alpha_vector[1] * total_fz); + + hrp::Vector3 tau_0 = hrp::Vector3::Zero(); +#if 0 + double gamma = fz_alpha; + double beta = m_wrenches[1].data[2] / ( m_wrenches[0].data[2] + m_wrenches[1].data[2] ); + beta = isnan(beta) ? 0 : beta; + double steepness = 8; // change ration from alpha to beta (steepness >= 4) + double r = - 1/(1+exp(-6*steepness*(gamma-1+1/steepness))) + 1/(1+exp(-6*steepness*(gamma-1/steepness))); + fz_alpha = r * beta + ( 1 - r ) * gamma; + // alpha = fz_alpha; + + ref_foot_force[0] = hrp::Vector3(0,0, fz_alpha * total_fz); + ref_foot_force[1] = hrp::Vector3(0,0, (1-fz_alpha) * total_fz); + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] slip st parameters" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] " << ref_foot_force[1](2) << " " << ref_foot_force[0](2) << " a:"<< steepness << " beta:" << beta << " gamma:" << gamma << " r:" << r << " fz_alpha:" << fz_alpha << " alpha:" << alpha << std::endl; + } +#endif + + for (size_t i = 0; i < 2; i++) { + tau_0 -= (cop_pos[i] - new_refzmp).cross(ref_foot_force[i]); + } + { + // Foot-distribution-coords frame => +// hrp::Vector3 foot_dist_coords_y = (cop_pos[1] - cop_pos[0]); // e_y' +// foot_dist_coords_y(2) = 0.0; +// foot_dist_coords_y.normalize(); +// hrp::Vector3 foot_dist_coords_x = hrp::Vector3(foot_dist_coords_y.cross(hrp::Vector3::UnitZ())); // e_x' +// hrp::Matrix33 foot_dist_coords_rot; +// foot_dist_coords_rot(0,0) = foot_dist_coords_x(0); +// foot_dist_coords_rot(1,0) = foot_dist_coords_x(1); +// foot_dist_coords_rot(2,0) = foot_dist_coords_x(2); +// foot_dist_coords_rot(0,1) = foot_dist_coords_y(0); +// foot_dist_coords_rot(1,1) = foot_dist_coords_y(1); +// foot_dist_coords_rot(2,1) = foot_dist_coords_y(2); +// foot_dist_coords_rot(0,2) = 0; +// foot_dist_coords_rot(1,2) = 0; +// foot_dist_coords_rot(2,2) = 1; +// hrp::Vector3 tau_0_f = foot_dist_coords_rot.transpose() * tau_0; // tau_0' +// // x +// // // right +// // if (tau_0_f(0) > 0) ref_foot_moment[0](0) = tau_0_f(0); +// // else ref_foot_moment[0](0) = 0; +// // // left +// // if (tau_0_f(0) > 0) ref_foot_moment[1](0) = 0; +// // else ref_foot_moment[1](0) = tau_0_f(0); +// ref_foot_moment[0](0) = tau_0_f(0) * alpha; +// ref_foot_moment[1](0) = tau_0_f(0) * (1-alpha); +// // y +// ref_foot_moment[0](1) = tau_0_f(1) * alpha; +// ref_foot_moment[1](1) = tau_0_f(1) * (1-alpha); +// ref_foot_moment[0](2) = ref_foot_moment[1](2) = 0.0; +// // <= Foot-distribution-coords frame +// // Convert foot-distribution-coords frame => actual world frame +// ref_foot_moment[0] = foot_dist_coords_rot * ref_foot_moment[0]; +// ref_foot_moment[1] = foot_dist_coords_rot * ref_foot_moment[1]; + // + ref_foot_moment[0](0) = tau_0(0) * alpha_vector[0]; + ref_foot_moment[1](0) = tau_0(0) * alpha_vector[1]; + ref_foot_moment[0](1) = tau_0(1) * alpha_vector[0]; + ref_foot_moment[1](1) = tau_0(1) * alpha_vector[1]; + ref_foot_moment[0](2) = ref_foot_moment[1](2)= 0.0; + } +#if 0 + { + // Foot-distribution-coords frame => + hrp::Vector3 foot_dist_coords_y = (cop_pos[1] - cop_pos[0]); // e_y' + foot_dist_coords_y(2) = 0.0; + foot_dist_coords_y.normalize(); + hrp::Vector3 foot_dist_coords_x = hrp::Vector3(foot_dist_coords_y.cross(hrp::Vector3::UnitZ())); // e_x' + hrp::Matrix33 foot_dist_coords_rot; + foot_dist_coords_rot(0,0) = foot_dist_coords_x(0); + foot_dist_coords_rot(1,0) = foot_dist_coords_x(1); + foot_dist_coords_rot(2,0) = foot_dist_coords_x(2); + foot_dist_coords_rot(0,1) = foot_dist_coords_y(0); + foot_dist_coords_rot(1,1) = foot_dist_coords_y(1); + foot_dist_coords_rot(2,1) = foot_dist_coords_y(2); + foot_dist_coords_rot(0,2) = 0; + foot_dist_coords_rot(1,2) = 0; + foot_dist_coords_rot(2,2) = 1; + tau_0 = hrp::Vector3::Zero(); + // + hrp::dvector fvec(3); + fvec(0) = total_fz; + fvec(1) = tau_0(0); + fvec(2) = tau_0(1); + hrp::dmatrix Gmat(3,6); + Gmat(0,0) = 1.0; Gmat(0,1) = 0.0; Gmat(0,2) = 0.0; + Gmat(0,3) = 1.0; Gmat(0,4) = 0.0; Gmat(0,5) = 0.0; + Gmat(1,0) = (cop_pos[0](1)-new_refzmp(1)); + Gmat(1,1) = 1.0; + Gmat(1,2) = 0.0; + Gmat(1,3) = (cop_pos[1](1)-new_refzmp(1)); + Gmat(1,4) = 1.0; + Gmat(1,5) = 0.0; + Gmat(2,0) = -(cop_pos[0](0)-new_refzmp(0)); + Gmat(2,1) = 0.0; + Gmat(2,2) = 1.0; + Gmat(2,3) = -(cop_pos[1](0)-new_refzmp(0)); + Gmat(2,4) = 0.0; + Gmat(2,5) = 1.0; + hrp::dmatrix Wmat(6,6); + for (size_t i = 0; i < 6; i++) { + for (size_t j = 0; j < 6; j++) { + Wmat(i,j) = 0.0; + } + } + double beta_r =0 , beta_l =0; + double kk = 8.0; + double alpha_r = 0.9; + double alpha_l = 0.1; + if (alpha > alpha_r) beta_r = std::pow((alpha/alpha_r-1.0), kk)/std::pow((1.0/alpha_r-1.0), kk); + else beta_r = 0; + if (alpha < alpha_l) beta_l = std::pow((alpha/alpha_l-1.0), kk); + else beta_l = 0; + Wmat(0,0) = alpha; + Wmat(1,1) = beta_r; + Wmat(2,2) = alpha; + Wmat(3,3) = (1-alpha); + Wmat(5,5) = (1-alpha); + Wmat(4,4) = beta_l; + // if (printp) { + // std::cerr << Wmat << std::endl; + // } + hrp::dmatrix Gmat_ret(6,3); + hrp::calcSRInverse(Gmat, Gmat_ret, 0.0, Wmat); + hrp::dvector fmvec(6); + fmvec = Gmat_ret* fvec; + ref_foot_force[0] = hrp::Vector3(0,0,fmvec(0)); + ref_foot_force[1] = hrp::Vector3(0,0,fmvec(3)); + ref_foot_moment[0] = hrp::Vector3(fmvec(1),fmvec(2),0); + ref_foot_moment[1] = hrp::Vector3(fmvec(4),fmvec(5),0); + // <= Foot-distribution-coords frame + // Convert foot-distribution-coords frame => actual world frame + ref_foot_moment[0] = foot_dist_coords_rot * ref_foot_moment[0]; + ref_foot_moment[1] = foot_dist_coords_rot * ref_foot_moment[1]; + } +#endif + + if (printp) { + //std::cerr << "[" << print_str << "] alpha = " << alpha << ", fz_alpha = " << fz_alpha << std::endl; + std::cerr << "[" << print_str << "] " + << "total_tau = " << hrp::Vector3(tau_0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_force_R = " << hrp::Vector3(ref_foot_force[0]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_force_L = " << hrp::Vector3(ref_foot_force[1]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_moment_R = " << hrp::Vector3(ref_foot_moment[0]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_moment_L = " << hrp::Vector3(ref_foot_moment[1]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + }; + +#ifdef USE_QPOASES + void solveForceMomentQPOASES (std::vector& fret, + const size_t state_dim, + const size_t ee_num, + const hrp::dmatrix& Hmat, + const hrp::dvector& gvec) + { + real_t* H = new real_t[state_dim*state_dim]; + real_t* g = new real_t[state_dim]; + real_t* lb = new real_t[state_dim]; + real_t* ub = new real_t[state_dim]; + for (size_t i = 0; i < state_dim; i++) { + for (size_t j = 0; j < state_dim; j++) { + H[i*state_dim+j] = Hmat(i,j); + } + g[i] = gvec(i); + lb[i] = 0.0; + ub[i] = 1e10; + } + QProblemB example( state_dim ); + Options options; + //options.enableFlippingBounds = BT_FALSE; + options.initialStatusBounds = ST_INACTIVE; + options.numRefinementSteps = 1; + options.enableCholeskyRefactorisation = 1; + //options.printLevel = PL_LOW; + options.printLevel = PL_NONE; + example.setOptions( options ); + /* Solve first QP. */ + int nWSR = 10; + example.init( H,g,lb,ub, nWSR,0 ); + real_t* xOpt = new real_t[state_dim]; + example.getPrimalSolution( xOpt ); + size_t state_dim_one = state_dim / ee_num; + for (size_t fidx = 0; fidx < ee_num; fidx++) { + for (size_t i = 0; i < state_dim_one; i++) { + fret[fidx](i) = xOpt[i+state_dim_one*fidx]; + } + } + delete[] H; + delete[] g; + delete[] lb; + delete[] ub; + delete[] xOpt; + }; + + void distributeZMPToForceMomentsQP (std::vector& ref_foot_force, std::vector& ref_foot_moment, + const std::vector& ee_pos, + const std::vector& cop_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const std::vector& limb_gains, + const std::vector& toeheel_ratio, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, + const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "", + const bool use_cop_distribution = false) + { + size_t ee_num = ee_name.size(); + std::vector alpha_vector(ee_num), fz_alpha_vector(ee_num); + if ( use_cop_distribution ) { + //calcAlphaVectorFromCOP(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp); + calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp); + } else { + calcAlphaVector(alpha_vector, fz_alpha_vector, ee_pos, ee_rot, ee_name, new_refzmp, ref_zmp); + } + + // QP + double norm_weight = 1e-7; + double cop_weight = 1e-3; + double ref_force_weight = 0;// 1e-3; + hrp::dvector total_fm(3); + total_fm(0) = total_fz; + total_fm(1) = 0; + total_fm(2) = 0; + size_t state_dim = 4*ee_num, state_dim_one = 4; // TODO + // + std::vector ff(ee_num, hrp::dvector(state_dim_one)); + std::vector mm(ee_num, hrp::dmatrix(3, state_dim_one)); + // + hrp::dmatrix Hmat = hrp::dmatrix::Zero(state_dim,state_dim); + hrp::dvector gvec = hrp::dvector::Zero(state_dim); + double alpha_thre = 1e-20; + // fz_alpha inversion for weighing matrix + for (size_t i = 0; i < fz_alpha_vector.size(); i++) { + fz_alpha_vector[i] *= limb_gains[i]; + fz_alpha_vector[i] = (fz_alpha_vector[i] < alpha_thre) ? 1/alpha_thre : 1/fz_alpha_vector[i]; + } + for (size_t j = 0; j < fz_alpha_vector.size(); j++) { + for (size_t i = 0; i < state_dim_one; i++) { + Hmat(i+j*state_dim_one,i+j*state_dim_one) = norm_weight * fz_alpha_vector[j]; + } + } + hrp::dmatrix Gmat(3,state_dim); + for (size_t i = 0; i < state_dim; i++) { + Gmat(0,i) = 1.0; + } + for (size_t fidx = 0; fidx < ee_num; fidx++) { + for (size_t i = 0; i < state_dim_one; i++) { + hrp::Vector3 fpos = ee_rot[fidx]*hrp::Vector3(fs.get_foot_vertex(fidx,i)(0), fs.get_foot_vertex(fidx,i)(1), 0) + ee_pos[fidx]; + mm[fidx](0,i) = 1.0; + mm[fidx](1,i) = -(fpos(1)-cop_pos[fidx](1)); + mm[fidx](2,i) = (fpos(0)-cop_pos[fidx](0)); + Gmat(1,i+state_dim_one*fidx) = -(fpos(1)-new_refzmp(1)); + Gmat(2,i+state_dim_one*fidx) = (fpos(0)-new_refzmp(0)); + } + //std::cerr << "fpos " << fpos[0] << " " << fpos[1] << std::endl; + } + Hmat += Gmat.transpose() * Gmat; + gvec += -1 * Gmat.transpose() * total_fm; + // std::cerr << "Gmat " << std::endl; + // std::cerr << Gmat << std::endl; + // std::cerr << "total_fm " << std::endl; + // std::cerr << total_fm << std::endl; + // + { + hrp::dmatrix Kmat = hrp::dmatrix::Zero(ee_num,state_dim); + hrp::dmatrix KW = hrp::dmatrix::Zero(ee_num, ee_num); + hrp::dvector reff(ee_num); + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < state_dim_one; i++) { + Kmat(j,i+j*state_dim_one) = 1.0; + } + reff(j) = ref_foot_force[j](2);// total_fz/2.0; + KW(j,j) = ref_force_weight; + } + Hmat += Kmat.transpose() * KW * Kmat; + gvec += -1 * Kmat.transpose() * KW * reff; + } + { + hrp::dmatrix Cmat = hrp::dmatrix::Zero(ee_num*2,state_dim); + hrp::dmatrix CW = hrp::dmatrix::Zero(ee_num*2,ee_num*2); + hrp::Vector3 fpos; + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < state_dim_one; i++) { + fpos = ee_rot[j]*hrp::Vector3(fs.get_foot_vertex(j,i)(0), fs.get_foot_vertex(j,i)(1), 0) + ee_pos[j]; + Cmat(j*2, i+j*state_dim_one) = fpos(0) - cop_pos[j](0); + Cmat(j*2+1,i+j*state_dim_one) = fpos(1) - cop_pos[j](1); + } + CW(j*2,j*2) = CW(j*2+1,j*2+1) = cop_weight; + } + Hmat += Cmat.transpose() * CW * Cmat; + } + // std::cerr << "H " << Hmat << std::endl; + // std::cerr << "g " << gvec << std::endl; + solveForceMomentQPOASES(ff, state_dim, ee_num, Hmat, gvec); + hrp::dvector tmpv(3); + for (size_t fidx = 0; fidx < ee_num; fidx++) { + tmpv = mm[fidx] * ff[fidx]; + ref_foot_force[fidx] = hrp::Vector3(0,0,tmpv(0)); + ref_foot_moment[fidx] = -1*hrp::Vector3(tmpv(1),tmpv(2),0); + } + if (printp) { + std::cerr << "[" << print_str << "] force moment distribution " << (use_cop_distribution ? "(QP COP)" : "(QP)") << std::endl; + //std::cerr << "[" << print_str << "] alpha = " << alpha << ", fz_alpha = " << fz_alpha << std::endl; + // std::cerr << "[" << print_str << "] " + // << "total_tau = " << hrp::Vector3(tau_0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl; + std::cerr << "[" << print_str << "] " + << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + } + }; +#else + void distributeZMPToForceMomentsQP (std::vector& ref_foot_force, std::vector& ref_foot_moment, + const std::vector& ee_pos, + const std::vector& cop_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const std::vector& limb_gains, + const std::vector& toeheel_ratio, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, + const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "", + const bool use_cop_distribution = false) + { + distributeZMPToForceMoments(ref_foot_force, ref_foot_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, toeheel_ratio, + new_refzmp, ref_zmp, + total_fz, dt, printp, print_str); + }; +#endif // USE_QPOASES + + // Solve A * x = b => x = W A^T (A W A^T)-1 b + // => x = W^{1/2} Pinv(A W^{1/2}) b + void calcWeightedLinearEquation(hrp::dvector& ret, const hrp::dmatrix& A, const hrp::dmatrix& W, const hrp::dvector& b) + { + hrp::dmatrix W2 = hrp::dmatrix::Zero(W.rows(), W.cols()); + for (size_t i = 0; i < W.rows(); i++) W2(i,i) = std::sqrt(W(i,i)); + hrp::dmatrix Aw = A*W2; + hrp::dmatrix Aw_inv = hrp::dmatrix::Zero(A.cols(), A.rows()); + hrp::calcPseudoInverse(Aw, Aw_inv); + ret = W2 * Aw_inv * b; + //ret = W2 * Aw.colPivHouseholderQr().solve(b); + }; + + void distributeZMPToForceMomentsPseudoInverse (std::vector& ref_foot_force, std::vector& ref_foot_moment, + const std::vector& ee_pos, + const std::vector& cop_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const std::vector& limb_gains, + const std::vector& toeheel_ratio, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, + const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "", + const bool use_cop_distribution = true, const std::vector is_contact_list = std::vector()) + { + size_t ee_num = ee_name.size(); + std::vector alpha_vector(ee_num), fz_alpha_vector(ee_num); + calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp); + if (printp) { + std::cerr << "[" << print_str << "] force moment distribution (Pinv)" << std::endl; + } + + // ref_foot_force and ref_foot_moment should be set + double norm_weight = 1e-7; + double norm_moment_weight = 1e2; + size_t total_wrench_dim = 5; + size_t state_dim = 6*ee_num; + + // Temporarily set ref_foot_force and ref_foot_moment based on reference values + { + size_t total_wrench_dim = 5; + //size_t total_wrench_dim = 3; + hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2); + hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim/2); + for (size_t j = 0; j < ee_num; j++) { + if (total_wrench_dim == 3) { + Gmat(0,3*j+2) = 1.0; + } else { + for (size_t k = 0; k < 3; k++) Gmat(k,3*j+k) = 1.0; + } + } + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < ee_num; j++) { + if ( i == total_wrench_dim-2 ) { // Nx + Gmat(i,3*j+1) = -(cop_pos[j](2) - ref_zmp(2)); + Gmat(i,3*j+2) = (cop_pos[j](1) - ref_zmp(1)); + } else if ( i == total_wrench_dim-1 ) { // Ny + Gmat(i,3*j) = (cop_pos[j](2) - ref_zmp(2)); + Gmat(i,3*j+2) = -(cop_pos[j](0) - ref_zmp(0)); + } + } + } + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + if (i != 2 && ee_num == 2) + Wmat(i+j*3, i+j*3) = 0; + else + Wmat(i+j*3, i+j*3) = Wmat(i+j*3, i+j*3) * limb_gains[j]; + } + } + if (printp) { + std::cerr << "[" << print_str << "] Wmat(diag) = ["; + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + std::cerr << Wmat(i+j*3, i+j*3) << " "; + } + } + std::cerr << "]" << std::endl; + } + hrp::dvector ret(state_dim/2); + hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim); + total_wrench(total_wrench_dim-3) = total_fz; + calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench); + for (size_t fidx = 0; fidx < ee_num; fidx++) { + ref_foot_force[fidx] = hrp::Vector3(ret(3*fidx), ret(3*fidx+1), ret(3*fidx+2)); + ref_foot_moment[fidx] = hrp::Vector3::Zero(); + } + // std::cerr << "GmatRef" << std::endl; + // std::cerr << Gmat << std::endl; + // std::cerr << "WmatRef" << std::endl; + // std::cerr << Wmat << std::endl; + // std::cerr << "ret" << std::endl; + // std::cerr << ret << std::endl; + } + if (printp) { + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + } + + // Calculate force/moment distribution matrix and vector + // We assume F = G f, calculate F and G. f is absolute here. + // 1. Calculate F (total_wrench) + hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim); + hrp::Vector3 ref_total_force = hrp::Vector3::Zero(); + for (size_t fidx = 0; fidx < ee_num; fidx++) { + double tmp_tau_x = -(cop_pos[fidx](2)-ref_zmp(2)) * ref_foot_force[fidx](1) + + (cop_pos[fidx](1)-ref_zmp(1)) * ref_foot_force[fidx](2) + + ref_foot_moment[fidx](0); + total_wrench(3) -= tmp_tau_x; + double tmp_tau_y = (cop_pos[fidx](2)-ref_zmp(2)) * ref_foot_force[fidx](0) + - (cop_pos[fidx](0)-ref_zmp(0)) * ref_foot_force[fidx](2) + + ref_foot_moment[fidx](1); + total_wrench(4) -= tmp_tau_y; + ref_total_force += ref_foot_force[fidx]; + } + total_wrench(3) -= -(ref_zmp(2) - new_refzmp(2)) * ref_total_force(1) + (ref_zmp(1) - new_refzmp(1)) * ref_total_force(2); + total_wrench(4) -= (ref_zmp(2) - new_refzmp(2)) * ref_total_force(0) - (ref_zmp(0) - new_refzmp(0)) * ref_total_force(2); + // 2. Calculate G (Gmat) + hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim); + for (size_t j = 0; j < ee_num; j++) { + for (size_t k = 0; k < total_wrench_dim; k++) Gmat(k,6*j+k) = 1.0; + } + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < ee_num; j++) { + if ( i == 3 ) { // Nx + Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2)); + Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1)); + } else if ( i == 4) { // Ny + Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2)); + Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0)); + } + } + } + // Calc rotation matrix to introduce local frame + // G = [tmpsubG_0, tmpsubG_1, ...] + // R = diag[tmpR_0, tmpR_1, ...] + // f = R f_l + // f : absolute, f_l : local + // G f = G R f_l + // G R = [tmpsubG_0 tmpR_0, tmpsubG_1 tmpR_1, ...] -> inserted to Gmat + hrp::dmatrix tmpsubG = hrp::dmatrix::Zero(total_wrench_dim, 6); + hrp::dmatrix tmpR = hrp::dmatrix::Zero(6,6); + for (size_t ei = 0; ei < ee_num; ei++) { + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < 6; j++) { + tmpsubG(i,j) = Gmat(i,6*ei+j); + } + } + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + tmpR(i,j) = tmpR(i+3,j+3) = ee_rot[ei](i,j); + } + } + tmpsubG = tmpsubG * tmpR; + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < 6; j++) { + Gmat(i,6*ei+j) = tmpsubG(i,j); + } + } + } + + // Calc weighting matrix + hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim); + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + Wmat(i+j*6, i+j*6) = fz_alpha_vector[j] * limb_gains[j]; + Wmat(i+j*6+3, i+j*6+3) = (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j]; + // Set local Y moment + // If toeheel_ratio is 0, toe and heel contact and local Y moment should be 0. + if (i == 1) { + Wmat(i+j*6+3, i+j*6+3) = toeheel_ratio[j] * Wmat(i+j*6+3, i+j*6+3); + } + // In actual swing phase, X/Y momoment should be 0. + if (!is_contact_list.empty()) { + if (!is_contact_list[j]) Wmat(i+j*6+3, i+j*6+3) = 0; + } + } + } + if (printp) { + std::cerr << "[" << print_str << "] newWmat(diag) = ["; + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 6; i++) { + std::cerr << Wmat(i+j*6, i+j*6) << " "; + } + } + std::cerr << "]" << std::endl; + } + // std::cerr << "total_wrench" << std::endl; + // std::cerr << total_wrench << std::endl; + // std::cerr << "Gmat" << std::endl; + // std::cerr << Gmat << std::endl; + // std::cerr << "Wmat" << std::endl; + // std::cerr << Wmat << std::endl; + + // Solve + hrp::dvector ret(state_dim); + calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench); + + // Extract force and moment with converting local frame -> absolute frame (ret is local frame) + for (size_t fidx = 0; fidx < ee_num; fidx++) { + ref_foot_force[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); + ref_foot_moment[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); + } + if (printp) { + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + } + }; + + void distributeZMPToForceMomentsPseudoInverse2 (std::vector& ref_foot_force, std::vector& ref_foot_moment, + const std::vector& ee_pos, + const std::vector& cop_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const std::vector& limb_gains, + const std::vector& toeheel_ratio, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, + const hrp::Vector3& total_force, const hrp::Vector3& total_moment, + const std::vector& ee_forcemoment_distribution_weight, + const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "") + { +#define FORCE_MOMENT_DIFF_CONTROL + + size_t ee_num = ee_name.size(); + std::vector alpha_vector(ee_num), fz_alpha_vector(ee_num); + calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp); + if (printp) { + std::cerr << "[" << print_str << "] force moment distribution (Pinv2) "; +#ifdef FORCE_MOMENT_DIFF_CONTROL + std::cerr << "(FORCE_MOMENT_DIFF_CONTROL)" << std::endl; +#else + std::cerr << "(NOT FORCE_MOMENT_DIFF_CONTROL)" << std::endl; +#endif + } + // ref_foot_force and ref_foot_moment should be set + size_t state_dim = 6*ee_num; + if (printp) { + std::cerr << "[" << print_str << "] fz_alpha_vector ["; + for (size_t j = 0; j < ee_num; j++) { + std::cerr << fz_alpha_vector[j] << " "; + } + std::cerr << std::endl; + } + + hrp::dvector total_wrench = hrp::dvector::Zero(6); +#ifndef FORCE_MOMENT_DIFF_CONTROL + total_wrench(0) = total_force(0); + total_wrench(1) = total_force(1); + total_wrench(2) = total_force(2); +#endif + total_wrench(3) = total_moment(0); + total_wrench(4) = total_moment(1); + total_wrench(5) = total_moment(2); +#ifdef FORCE_MOMENT_DIFF_CONTROL + for (size_t fidx = 0; fidx < ee_num; fidx++) { + double tmp_tau_x = -(cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](1) + (cop_pos[fidx](1)-new_refzmp(1)) * ref_foot_force[fidx](2); + total_wrench(3) -= tmp_tau_x; + double tmp_tau_y = (cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](0) - (cop_pos[fidx](0)-new_refzmp(0)) * ref_foot_force[fidx](2); + total_wrench(4) -= tmp_tau_y; + } +#endif + + hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim); + hrp::dmatrix Gmat = hrp::dmatrix::Zero(6, state_dim); + for (size_t j = 0; j < ee_num; j++) { + for (size_t k = 0; k < 6; k++) Gmat(k,6*j+k) = 1.0; + } + for (size_t i = 0; i < 6; i++) { + for (size_t j = 0; j < ee_num; j++) { + if ( i == 3 ) { // Nx + Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2)); + Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1)); + } else if ( i == 4 ) { // Ny + Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2)); + Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0)); + } + } + } + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + Wmat(i+j*6, i+j*6) = ee_forcemoment_distribution_weight[j][i] * fz_alpha_vector[j] * limb_gains[j]; + //double norm_moment_weight = 1e2; + //Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j]; + Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * fz_alpha_vector[j] * limb_gains[j]; + } + } + if (printp) { + std::cerr << "[" << print_str << "] newWmat(diag) = ["; + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 6; i++) { + std::cerr << Wmat(i+j*6, i+j*6) << " "; + } + } + std::cerr << std::endl; + // std::cerr << "[" << print_str << "] Gmat"; + // std::cerr << Gmat << std::endl; + // std::cerr << "Wmat" << std::endl; + // std::cerr << Wmat << std::endl; + } + + hrp::dvector ret(state_dim); + // Consider 6DOF total wrench (Fx, Fy, Fz, Mx, My, Mz) + hrp::dmatrix selection_matrix = hrp::dmatrix::Identity(6,6); + // Consdier 3DOF total wrench (Fz, Mx, My) +// hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(3,6); +// selection_matrix(0,2) = 1.0; +// selection_matrix(1,3) = 1.0; +// selection_matrix(2,4) = 1.0; + { + hrp::dvector selected_total_wrench = hrp::dvector::Zero(selection_matrix.rows()); + hrp::dmatrix selected_Gmat = hrp::dmatrix::Zero(selection_matrix.rows(), Gmat.cols()); + selected_total_wrench = selection_matrix * total_wrench; + selected_Gmat = selection_matrix * Gmat; + calcWeightedLinearEquation(ret, selected_Gmat, Wmat, selected_total_wrench); + } + + if (printp) { + hrp::dvector tmpretv(total_wrench.size()); + tmpretv = Gmat * ret - total_wrench; + std::cerr << "[" << print_str << "] " + << "test_diff " << tmpretv.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl; + std::cerr << "[" << print_str << "] " + << "total_wrench " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl; + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + for (size_t i = 0; i < ee_num; i++) { +#ifdef FORCE_MOMENT_DIFF_CONTROL + std::cerr << "[" << print_str << "] " + << "dif_ref_force [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; +#else + std::cerr << "[" << print_str << "] " + << "dif_ref_force [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))-ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))-ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; +#endif + } + } + for (size_t fidx = 0; fidx < ee_num; fidx++) { +#ifdef FORCE_MOMENT_DIFF_CONTROL + ref_foot_force[fidx] += hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); + ref_foot_moment[fidx] += hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); +#else + ref_foot_force[fidx] = hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); + ref_foot_moment[fidx] = hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); +#endif + } + if (printp){ + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + } + }; + + double calcCrossProduct(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& o) + { + return (a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0)); + }; + + projected_point_region calcProjectedPoint(Eigen::Vector2d& ret, const Eigen::Vector2d& target, const Eigen::Vector2d& a, const Eigen::Vector2d& b){ + Eigen::Vector2d v1 = target - b; + Eigen::Vector2d v2 = a - b; + double v2_norm = v2.norm(); + if ( v2_norm == 0 ) { + ret = a; + return LEFT; + } else { + double ratio = v1.dot(v2) / (v2_norm * v2_norm); + if (ratio < 0){ + ret = b; + return RIGHT; + } else if (ratio > 1){ + ret = a; + return LEFT; + } else { + ret = b + ratio * v2; + return MIDDLE; + } + } + }; +}; + +#endif // ZMP_DISTRIBUTOR_H diff --git a/rtc/AutoBalancer/testGaitGenerator.cpp b/rtc/AutoBalancer/testGaitGenerator.cpp index 0289014217e..832eee4b9f5 100644 --- a/rtc/AutoBalancer/testGaitGenerator.cpp +++ b/rtc/AutoBalancer/testGaitGenerator.cpp @@ -1,12 +1,101 @@ /* -*- coding:utf-8-unix; mode:c++; -*- */ #include "GaitGenerator.h" -/* samples */ +/*! + * @file testGaitGenerator.cpp + * @brief Test of gait generator + * @date $Date$ + * + * $Id$ + * TODO + * Support test of quad walking + * Support test of foot_dif_angle + * Support footstep modification using capture point feedback + * Support TODO in check_end_values + * Supprot other robots and dt + */ + + using namespace rats; #include #include #define eps_eq(a,b,epsilon) (std::fabs((a)-(b)) < (epsilon)) +#ifndef rad2deg +#define rad2deg(rad) (rad * 180 / M_PI) +#endif +#ifndef deg2rad +#define deg2rad(deg) (deg * M_PI / 180) +#endif + +// Difference checker to observe too large discontinuous values +// isSmallDiff is true, calculation is correct and continuous. +template class ValueDifferenceChecker +{ + T prev_value; + double diff_thre, max_value_diff; + bool is_small_diff, is_initialized; + double calcDiff (T& value) const { return 0; }; +public: + ValueDifferenceChecker (double _diff_thre) : diff_thre (_diff_thre), max_value_diff(0), is_small_diff(true), is_initialized(false) + { + }; + ~ValueDifferenceChecker () {}; + void checkValueDiff (T& value) + { + // Initialize prev value + if (!is_initialized) { + prev_value = value; + is_initialized = true; + } + // Calc diff and update + double diff = calcDiff(value); + if (diff > max_value_diff) max_value_diff = diff; + is_small_diff = (diff < diff_thre) && is_small_diff; + prev_value = value; + }; + double getMaxValue () const { return max_value_diff; }; + double getDiffThre () const { return diff_thre; }; + bool isSmallDiff () const { return is_small_diff; }; +}; + +template<> double ValueDifferenceChecker::calcDiff (hrp::Vector3& value) const +{ + return (value - prev_value).norm(); +}; + +template<> double ValueDifferenceChecker< std::vector >::calcDiff (std::vector& value) const +{ + double tmp = 0; + for (size_t i = 0; i < value.size(); i++) { + tmp += (value[i] - prev_value[i]).norm(); + } + return tmp; +}; + +// Error checker between two input values +// isSmallError is true, error is correct. +class ValueErrorChecker +{ + double error_thre, max_value_error; + bool is_small_error; +public: + ValueErrorChecker (double _thre) : error_thre (_thre), max_value_error(0), is_small_error(true) + { + }; + ~ValueErrorChecker () {}; + void checkValueError (const hrp::Vector3& p0, const hrp::Vector3& p1, std::vector neglect_index = std::vector()) + { + hrp::Vector3 errorv(p0-p1); + for (size_t i = 0; i < neglect_index.size(); i++) errorv(neglect_index[i]) = 0.0; + double error = errorv.norm(); + if (error > max_value_error) max_value_error = error; + is_small_error = (error < error_thre) && is_small_error; + }; + double getMaxValue () const { return max_value_error; }; + double getErrorThre () const { return error_thre; }; + bool isSmallError () const { return is_small_error; }; +}; class testGaitGenerator { @@ -16,18 +105,53 @@ class testGaitGenerator std::vector all_limbs; hrp::Vector3 cog; gait_generator* gg; - bool use_gnuplot, is_small_zmp_error, is_small_zmp_diff, is_contact_states_swing_support_time_validity; + bool use_gnuplot, use_graph_append; + // previous values for walk pattern calculation + hrp::Vector3 prev_rfoot_pos, prev_lfoot_pos, prev_rfoot_rpy, prev_lfoot_rpy; + hrp::Vector3 min_rfoot_pos, min_lfoot_pos, max_rfoot_pos, max_lfoot_pos; + hrp::Vector3 prev_refzmp; + coordinates prev_ssmc; + std::vector prev_contact_states; + std::vector prev_swing_support_time; + double min_toe_heel_dif_angle, max_toe_heel_dif_angle, min_zmp_offset_x, max_zmp_offset_x; + // Value checker + bool is_contact_states_swing_support_time_validity; + // Check difference of value + ValueDifferenceChecker< hrp::Vector3 > refzmp_diff_checker, cartzmp_diff_checker, cog_diff_checker, ssmcpos_diff_checker, ssmcrot_diff_checker, ssmcposvel_diff_checker, ssmcrotvel_diff_checker; + ValueDifferenceChecker< std::vector > footpos_diff_checker, footrot_diff_checker, footposvel_diff_checker, footrotvel_diff_checker, zmpoffset_diff_checker; + // Check errors between two values + ValueErrorChecker zmp_error_checker, cogzmp_error_checker; + // Results of list of step time, toe/heel angle, and zmp offset + std::vector step_time_list, min_toe_heel_dif_angle_list, max_toe_heel_dif_angle_list, min_zmp_offset_x_list, max_zmp_offset_x_list; + bool is_step_time_valid, is_toe_heel_dif_angle_valid, is_toe_heel_zmp_offset_x_valid; + // For plot + std::string test_doc_string; + std::string fname_cogzmp; + FILE* fp_cogzmp; + std::string fname_fpos; + FILE* fp_fpos; + std::string fname_frot; + FILE* fp_frot; + std::string fname_zoff; + FILE* fp_zoff; + std::string fname_fposvel; + FILE* fp_fposvel; + std::string fname_frotvel; + FILE* fp_frotvel; + std::string fname_thpos; + FILE* fp_thpos; + std::string fname_sstime; + FILE* fp_sstime; + std::string fname_ssmc; + FILE* fp_ssmc; + std::string fname_ssmcvel; + FILE* fp_ssmcvel; private: - // error check - bool check_zmp_error (const hrp::Vector3& czmp, const hrp::Vector3& refzmp) - { - return (czmp-refzmp).norm() < 50.0*1e-3; // [mm] - } - bool check_zmp_diff (const hrp::Vector3& prev_zmp, const hrp::Vector3& zmp) - { - return (prev_zmp - zmp).norm() < 10.0*1e-3; // [mm] - } + //// // plot and pattern generation + //// + + // Plot gnuplot graph and and save graphs to eps files void plot_and_save (FILE* gp, const std::string graph_fname, const std::string plot_str) { fprintf(gp, "%s\n unset multiplot\n", plot_str.c_str()); @@ -35,34 +159,25 @@ class testGaitGenerator fprintf(gp, "%s\n unset multiplot\n", plot_str.c_str()); fflush(gp); }; - void plot_walk_pattern () + + // Dump generated motion by proc_one_tick function. + // Calculate error checking and difference values + void proc_one_walking_motion (size_t i) { - /* make step and dump */ - size_t i = 0; - std::string fname("/tmp/plot.dat"); - FILE* fp = fopen(fname.c_str(), "w"); - std::string fname_sstime("/tmp/plot-sstime.dat"); - FILE* fp_sstime = fopen(fname_sstime.c_str(), "w"); - hrp::Vector3 prev_rfoot_pos, prev_lfoot_pos; - hrp::Vector3 min_rfoot_pos(1e10,1e10,1e10), min_lfoot_pos(1e10,1e10,1e10), max_rfoot_pos(-1e10,-1e10,-1e10), max_lfoot_pos(-1e10,-1e10,-1e10); - // - hrp::Vector3 prev_refzmp; - std::vector tmp_string_vector; - std::vector prev_contact_states(2, true); // RLEG, LLEG - std::vector prev_swing_support_time(2, 1e2); // RLEG, LLEG - while ( gg->proc_one_tick() ) { //std::cerr << gg->lcg.gp_count << std::endl; // if ( gg->lcg.gp_index == 4 && gg->lcg.gp_count == 100) { // //std::cerr << gg->lcg.gp_index << std::endl; // gg->update_refzmp_queue(coordinates(hrp::Vector3(150, 105, 0)), coordinates(hrp::Vector3(150, -105, 0))); // } - fprintf(fp, "%f ", i * dt); + + // COG and ZMP + fprintf(fp_cogzmp, "%f ", i * dt); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", gg->get_refzmp()(ii)); + fprintf(fp_cogzmp, "%f ", gg->get_refzmp()(ii)); } hrp::Vector3 czmp = gg->get_cart_zmp(); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", czmp(ii)); + fprintf(fp_cogzmp, "%f ", czmp(ii)); } for (size_t ii = 0; ii < 3; ii++) { double cogpos; @@ -73,101 +188,223 @@ class testGaitGenerator } else { cogpos = gg->get_cog()(ii); } - fprintf(fp, "%f ", cogpos); + fprintf(fp_cogzmp, "%f ", cogpos); } + fprintf(fp_cogzmp, "\n"); + fflush(fp_cogzmp); + +#define VEC1(s) std::vector (1, s) + // Foot pos - tmp_string_vector = boost::assign::list_of("rleg"); - hrp::Vector3 rfoot_pos = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos; + fprintf(fp_fpos, "%f ", i * dt); + hrp::Vector3 rfoot_pos = (gg->get_support_leg_names() == VEC1 ("rleg")) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos; for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", rfoot_pos(ii)); + fprintf(fp_fpos, "%f ", rfoot_pos(ii)); min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), rfoot_pos(ii)); max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), rfoot_pos(ii)); } - tmp_string_vector = boost::assign::list_of("lleg"); - hrp::Vector3 lfoot_pos = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos; + hrp::Vector3 lfoot_pos = (gg->get_support_leg_names() == VEC1("lleg")) ? gg->get_support_leg_steps().front().worldcoords.pos : gg->get_swing_leg_steps().front().worldcoords.pos; for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", lfoot_pos(ii)); + fprintf(fp_fpos, "%f ", lfoot_pos(ii)); min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), lfoot_pos(ii)); max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), lfoot_pos(ii)); } + fprintf(fp_fpos, "\n"); + fflush(fp_fpos); + // Foot rot - hrp::Vector3 rpy; - tmp_string_vector = boost::assign::list_of("rleg"); - hrp::Matrix33 rfoot_rot = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot; - rpy = hrp::rpyFromRot(rfoot_rot); + fprintf(fp_frot, "%f ", i * dt); + hrp::Matrix33 rfoot_rot = (gg->get_support_leg_names() == VEC1("rleg")) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot; + hrp::Vector3 rfoot_rpy = hrp::rpyFromRot(rfoot_rot); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", 180.0*rpy(ii)/M_PI); + fprintf(fp_frot, "%f ", rad2deg(rfoot_rpy(ii))); } - tmp_string_vector = boost::assign::list_of("lleg"); - hrp::Matrix33 lfoot_rot = (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot; - rpy = hrp::rpyFromRot(lfoot_rot); + hrp::Matrix33 lfoot_rot = (gg->get_support_leg_names() == VEC1("lleg")) ? gg->get_support_leg_steps().front().worldcoords.rot : gg->get_swing_leg_steps().front().worldcoords.rot; + hrp::Vector3 lfoot_rpy = hrp::rpyFromRot(lfoot_rot); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", 180.0*rpy(ii)/M_PI); + fprintf(fp_frot, "%f ", rad2deg(lfoot_rpy(ii))); } + fprintf(fp_frot, "\n"); + fflush(fp_frot); + // ZMP offsets - tmp_string_vector = boost::assign::list_of("rleg"); + fprintf(fp_zoff, "%f ", i * dt); + hrp::Vector3 rfoot_zmp_offset = (gg->get_support_leg_names() == VEC1("rleg")) ? gg->get_support_foot_zmp_offsets().front() : gg->get_swing_foot_zmp_offsets().front(); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_foot_zmp_offsets().front()(ii) : gg->get_swing_foot_zmp_offsets().front()(ii)); + fprintf(fp_zoff, "%f ", rfoot_zmp_offset(ii)); } - tmp_string_vector = boost::assign::list_of("lleg"); + hrp::Vector3 lfoot_zmp_offset = (gg->get_support_leg_names() == VEC1("lleg")) ? gg->get_support_foot_zmp_offsets().front() : gg->get_swing_foot_zmp_offsets().front(); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", (gg->get_support_leg_names() == tmp_string_vector) ? gg->get_support_foot_zmp_offsets().front()(ii) : gg->get_swing_foot_zmp_offsets().front()(ii)); + fprintf(fp_zoff, "%f ", lfoot_zmp_offset(ii)); } - // Foot vel - hrp::Vector3 tmpv; + fprintf(fp_zoff, "\n"); + fflush(fp_zoff); + double tmpzoff; + if (gg->get_support_leg_names()[0] == "lleg") tmpzoff = rfoot_zmp_offset(0); + else tmpzoff = lfoot_zmp_offset(0); + min_zmp_offset_x = std::min(min_zmp_offset_x, tmpzoff); + max_zmp_offset_x = std::max(max_zmp_offset_x, tmpzoff); + +#undef VEC1 + + // Foot pos vel + fprintf(fp_fposvel, "%f ", i * dt); if ( i == 0 ) prev_rfoot_pos = rfoot_pos; - tmpv = (rfoot_pos - prev_rfoot_pos)/dt; + hrp::Vector3 rfootpos_vel = (rfoot_pos - prev_rfoot_pos)/dt; for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", tmpv(ii)); + fprintf(fp_fposvel, "%f ", rfootpos_vel(ii)); } prev_rfoot_pos = rfoot_pos; if ( i == 0 ) prev_lfoot_pos = lfoot_pos; - tmpv = (lfoot_pos - prev_lfoot_pos)/dt; + hrp::Vector3 lfootpos_vel = (lfoot_pos - prev_lfoot_pos)/dt; for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", tmpv(ii)); + fprintf(fp_fposvel, "%f ", lfootpos_vel(ii)); } prev_lfoot_pos = lfoot_pos; + fprintf(fp_fposvel, "\n"); + fflush(fp_fposvel); + + // Foot rot vel + fprintf(fp_frotvel, "%f ", i * dt); + if ( i == 0 ) prev_rfoot_rpy = rfoot_rpy; + hrp::Vector3 rfootrot_vel = (rfoot_rpy - prev_rfoot_rpy)/dt; + for (size_t ii = 0; ii < 3; ii++) { + fprintf(fp_frotvel, "%f ", rfootrot_vel(ii)); + } + prev_rfoot_rpy = rfoot_rpy; + if ( i == 0 ) prev_lfoot_rpy = lfoot_rpy; + hrp::Vector3 lfootrot_vel = (lfoot_rpy - prev_lfoot_rpy)/dt; + for (size_t ii = 0; ii < 3; ii++) { + fprintf(fp_frotvel, "%f ", lfootrot_vel(ii)); + } + prev_lfoot_rpy = lfoot_rpy; + fprintf(fp_frotvel, "\n"); + fflush(fp_frotvel); + // Toe heel pos + fprintf(fp_thpos, "%f ", i * dt); hrp::Vector3 tmppos; tmppos = rfoot_pos+rfoot_rot*hrp::Vector3(gg->get_toe_pos_offset_x(), 0, 0); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", tmppos(ii)); + fprintf(fp_thpos, "%f ", tmppos(ii)); min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), tmppos(ii)); max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), tmppos(ii)); } tmppos = lfoot_pos+lfoot_rot*hrp::Vector3(gg->get_toe_pos_offset_x(), 0, 0); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", tmppos(ii)); + fprintf(fp_thpos, "%f ", tmppos(ii)); min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), tmppos(ii)); max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), tmppos(ii)); } tmppos = rfoot_pos+rfoot_rot*hrp::Vector3(gg->get_heel_pos_offset_x(), 0, 0); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", tmppos(ii)); + fprintf(fp_thpos, "%f ", tmppos(ii)); min_rfoot_pos(ii) = std::min(min_rfoot_pos(ii), tmppos(ii)); max_rfoot_pos(ii) = std::max(max_rfoot_pos(ii), tmppos(ii)); } tmppos = lfoot_pos+lfoot_rot*hrp::Vector3(gg->get_heel_pos_offset_x(), 0, 0); for (size_t ii = 0; ii < 3; ii++) { - fprintf(fp, "%f ", tmppos(ii)); + fprintf(fp_thpos, "%f ", tmppos(ii)); min_lfoot_pos(ii) = std::min(min_lfoot_pos(ii), tmppos(ii)); max_lfoot_pos(ii) = std::max(max_lfoot_pos(ii), tmppos(ii)); } - fprintf(fp, "\n"); + fprintf(fp_thpos, "\n"); + fflush(fp_thpos); + // Swing time fprintf(fp_sstime, "%f ", i * dt); fprintf(fp_sstime, "%f %f ", gg->get_current_swing_time(RLEG), gg->get_current_swing_time(LLEG)); + // Contact States std::vector tmp_current_support_states = gg->get_current_support_states(); bool rleg_contact_states = std::find_if(tmp_current_support_states.begin(), tmp_current_support_states.end(), boost::lambda::_1 == RLEG) != tmp_current_support_states.end(); bool lleg_contact_states = std::find_if(tmp_current_support_states.begin(), tmp_current_support_states.end(), boost::lambda::_1 == LLEG) != tmp_current_support_states.end(); - fprintf(fp_sstime, "%d %d ", (rleg_contact_states ? 1 : 0), (lleg_contact_states ? 1 : 0)); + fprintf(fp_sstime, "%d %d %f", + (rleg_contact_states ? 1 : 0), (lleg_contact_states ? 1 : 0), + 0.8*gg->get_current_toe_heel_ratio()+0.1); // scale+translation just for visualization fprintf(fp_sstime, "\n"); + fflush(fp_sstime); + + // swing support mid coords + fprintf(fp_ssmc, "%f ", i * dt); + coordinates tmp_ssmc; + gg->get_swing_support_mid_coords(tmp_ssmc); + for (size_t ii = 0; ii < 3; ii++) { + fprintf(fp_ssmc, "%f ", tmp_ssmc.pos(ii)); + } + hrp::Vector3 tmp_ssmcr = hrp::rpyFromRot(tmp_ssmc.rot); + for (size_t ii = 0; ii < 3; ii++) { + fprintf(fp_ssmc, "%f ", rad2deg(tmp_ssmcr(ii))); + } + fprintf(fp_ssmc, "\n"); + fflush(fp_ssmc); + + // swing support mid coords vel + fprintf(fp_ssmcvel, "%f ", i * dt); + if ( i == 0 ) prev_ssmc = tmp_ssmc; + hrp::Vector3 tmp_ssmcpos_vel = (tmp_ssmc.pos - prev_ssmc.pos)/dt; + for (size_t ii = 0; ii < 3; ii++) { + fprintf(fp_ssmcvel, "%f ", tmp_ssmcpos_vel(ii)); + } + hrp::Vector3 prev_ssmcr = hrp::rpyFromRot(prev_ssmc.rot); + hrp::Vector3 tmp_ssmcrot_vel = (tmp_ssmcr - prev_ssmcr)/dt; + for (size_t ii = 0; ii < 3; ii++) { + fprintf(fp_ssmcvel, "%f ", tmp_ssmcrot_vel(ii)); + } + fprintf(fp_ssmcvel, "\n"); + fflush(fp_ssmcvel); + prev_ssmc = tmp_ssmc; + + // Toe heel angle + double tmp_toe_heel_dif_angle = gg->get_toe_heel_dif_angle(); + min_toe_heel_dif_angle = std::min(min_toe_heel_dif_angle, tmp_toe_heel_dif_angle); + max_toe_heel_dif_angle = std::max(max_toe_heel_dif_angle, tmp_toe_heel_dif_angle); + + // footstep_node change + if (gg->get_lcg_count() == 0) { + step_time_list.push_back(gg->get_one_step_count()*dt); + min_toe_heel_dif_angle_list.push_back(min_toe_heel_dif_angle); + max_toe_heel_dif_angle_list.push_back(max_toe_heel_dif_angle); + min_toe_heel_dif_angle = 1e10; + max_toe_heel_dif_angle = -1e10; + if ( !eps_eq(min_zmp_offset_x, gg->get_default_zmp_offsets()[0](0), 1e-5) ) min_zmp_offset_x_list.push_back(min_zmp_offset_x); + else min_zmp_offset_x_list.push_back(0.0); + if ( !eps_eq(max_zmp_offset_x, gg->get_default_zmp_offsets()[0](0), 1e-5) ) max_zmp_offset_x_list.push_back(max_zmp_offset_x); + else max_zmp_offset_x_list.push_back(0.0); + min_zmp_offset_x = 1e10; + max_zmp_offset_x = -1e10; + } + // Error checking - is_small_zmp_error = check_zmp_error(gg->get_cart_zmp(), gg->get_refzmp()) && is_small_zmp_error; - if (i>0) { - is_small_zmp_diff = check_zmp_diff(prev_refzmp, gg->get_refzmp()) && is_small_zmp_diff; + { + // Check error between RefZMP and CartZMP. If too large error, PreviewControl tracking is not enough. + zmp_error_checker.checkValueError(gg->get_cart_zmp(), gg->get_refzmp()); + // Check too large differences (discontinuity) + // COG and ZMP + hrp::Vector3 tmp(gg->get_refzmp()); + refzmp_diff_checker.checkValueDiff(tmp); + tmp = gg->get_cart_zmp(); + cartzmp_diff_checker.checkValueDiff(tmp); + tmp = gg->get_cog(); + cog_diff_checker.checkValueDiff(tmp); + // Foot pos and rot + std::vector tmpvec = boost::assign::list_of(rfoot_pos)(lfoot_pos); + footpos_diff_checker.checkValueDiff(tmpvec); + tmpvec = boost::assign::list_of(rfoot_rpy)(lfoot_rpy).convert_to_container < std::vector > (); + footrot_diff_checker.checkValueDiff(tmpvec); + tmpvec = boost::assign::list_of(rfootpos_vel)(lfootpos_vel).convert_to_container < std::vector > (); + footposvel_diff_checker.checkValueDiff(tmpvec); + tmpvec = boost::assign::list_of(rfootrot_vel)(lfootrot_vel).convert_to_container < std::vector > (); + footrotvel_diff_checker.checkValueDiff(tmpvec); + // Swing support mid coorsd + ssmcpos_diff_checker.checkValueDiff(tmp_ssmc.pos); + ssmcrot_diff_checker.checkValueDiff(tmp_ssmcr); + ssmcposvel_diff_checker.checkValueDiff(tmp_ssmcpos_vel); + ssmcrotvel_diff_checker.checkValueDiff(tmp_ssmcrot_vel); + // ZMP offset + tmpvec = boost::assign::list_of(rfoot_zmp_offset)(lfoot_zmp_offset).convert_to_container < std::vector > (); + zmpoffset_diff_checker.checkValueDiff(tmpvec); } // If contact states are not change, prev_swing_support_time is not dt, otherwise prev_swing_support_time is dt. is_contact_states_swing_support_time_validity = is_contact_states_swing_support_time_validity && @@ -178,15 +415,14 @@ class testGaitGenerator prev_contact_states[1] = lleg_contact_states; prev_swing_support_time[0] = gg->get_current_swing_time(RLEG); prev_swing_support_time[1] = gg->get_current_swing_time(LLEG); - i++; - } - fclose(fp); - fclose(fp_sstime); + }; + // Plot state values and print error check + void plot_and_print_errorcheck () + { /* plot */ if (use_gnuplot) { - size_t gpsize = 7; - size_t tmp_start = 2; + size_t gpsize = 12; FILE* gps[gpsize]; for (size_t ii = 0; ii < gpsize;ii++) { gps[ii] = popen("gnuplot", "w"); @@ -194,91 +430,112 @@ class testGaitGenerator { std::ostringstream oss(""); std::string gtitle("COG_and_ZMP"); + size_t tmp_start = 2; oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; std::string titles[3] = {"X", "Y", "Z"}; for (size_t ii = 0; ii < 3; ii++) { oss << "set xlabel 'Time [s]'" << std::endl; oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl; oss << "plot " - << "'" << fname << "' using 1:" << (tmp_start+ii) << " with lines title 'REFZMP'," - << "'" << fname << "' using 1:" << (tmp_start+3+ii) << " with lines title 'CARTZMP'," - << "'" << fname << "' using 1:" << (tmp_start+6+ii) << " with lines title 'COG'" + << "'" << fname_cogzmp << "' using 1:" << (tmp_start+ii) << " with lines title 'REFZMP'," + << "'" << fname_cogzmp << "' using 1:" << (tmp_start+3+ii) << " with lines title 'CARTZMP'," + << "'" << fname_cogzmp << "' using 1:" << (tmp_start+6+ii) << " with lines title 'COG'" << std::endl; } plot_and_save(gps[0], gtitle, oss.str()); - tmp_start += 9; } { std::ostringstream oss(""); std::string gtitle("Swing_support_pos"); + size_t tmp_start = 2; oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; std::string titles[3] = {"X", "Y", "Z"}; for (size_t ii = 0; ii < 3; ii++) { oss << "set xlabel 'Time [s]'" << std::endl; oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl; oss << "plot " - << "'" << fname << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," - << "'" << fname << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" + << "'" << fname_fpos << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," + << "'" << fname_fpos << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" << std::endl; } plot_and_save(gps[1], gtitle, oss.str()); - tmp_start += 6; } { std::ostringstream oss(""); std::string gtitle("Swing_support_rot"); + size_t tmp_start = 2; oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; std::string titles[3] = {"Roll", "Pitch", "Yaw"}; for (size_t ii = 0; ii < 3; ii++) { oss << "set xlabel 'Time [s]'" << std::endl; oss << "set ylabel '" << titles[ii] << "[deg]'" << std::endl; oss << "plot " - << "'" << fname << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," - << "'" << fname << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" + << "'" << fname_frot << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," + << "'" << fname_frot << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" << std::endl; } plot_and_save(gps[2], gtitle, oss.str()); - tmp_start += 6; } { std::ostringstream oss(""); std::string gtitle("Swing_support_zmp_offset"); + size_t tmp_start = 2; oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; std::string titles[3] = {"X", "Y", "Z"}; for (size_t ii = 0; ii < 3; ii++) { oss << "set xlabel 'Time [s]'" << std::endl; oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl; oss << "plot " - << "'" << fname << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," - << "'" << fname << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" + << "'" << fname_zoff << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," + << "'" << fname_zoff << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" << std::endl; } plot_and_save(gps[3], gtitle, oss.str()); - tmp_start += 6; } { std::ostringstream oss(""); - std::string gtitle("Swing_support_vel"); + std::string gtitle("Swing_support_pos_vel"); + size_t tmp_start = 2; oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; std::string titles[3] = {"X", "Y", "Z"}; for (size_t ii = 0; ii < 3; ii++) { oss << "set xlabel 'Time [s]'" << std::endl; - oss << "set ylabel '" << titles[ii] << "[m]'" << std::endl; + oss << "set ylabel '" << titles[ii] << "[m/s]'" << std::endl; oss << "plot " - << "'" << fname << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," - << "'" << fname << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" + << "'" << fname_fposvel << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," + << "'" << fname_fposvel << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" + << std::endl; + } + plot_and_save(gps[4], gtitle, oss.str()); + } + { + std::ostringstream oss(""); + std::string gtitle("Swing_support_rot_vel"); + size_t tmp_start = 2; + oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; + std::string titles[3] = {"Roll", "Pitch", "Yaw"}; + for (size_t ii = 0; ii < 3; ii++) { + oss << "set xlabel 'Time [s]'" << std::endl; + oss << "set ylabel '" << titles[ii] << "[deg/s]'" << std::endl; + oss << "plot " + << "'" << fname_frotvel << "' using 1:" << (tmp_start+ii) << " with lines title 'rleg'," + << "'" << fname_frotvel << "' using 1:" << (tmp_start+3+ii) << " with lines title 'lleg'" << std::endl; } plot_and_save(gps[5], gtitle, oss.str()); - tmp_start += 6; } { std::ostringstream oss(""); std::string gtitle("Swing_support_pos_trajectory"); double min_v[3], max_v[3], range[3]; + size_t tmp_thpos_start = 2; for (size_t ii = 0; ii < 3; ii++) { min_v[ii] = std::min(min_rfoot_pos(ii), min_lfoot_pos(ii)); max_v[ii] = std::max(max_rfoot_pos(ii), max_lfoot_pos(ii)); + if (min_v[ii] == 0.0 && max_v[ii] == 0.0) { + min_v[ii] = -0.1; + max_v[ii] = 0.1; + } range[ii] = max_v[ii] - min_v[ii]; double mid = (max_v[ii]+min_v[ii])/2.0; min_v[ii] = mid + range[ii] * 1.05 * -0.5; @@ -292,12 +549,12 @@ class testGaitGenerator oss << "plot " << "[" << min_v[0]<< ":" << max_v[0] << "]" << "[" << min_v[2] << ":" << max_v[2] << "]" - << "'" << fname << "' using " << (2+3+3+3+0) << ":" << (2+3+3+3+2) << " with lines title 'rleg ee'," - << "'" << fname << "' using " << (2+3+3+3+3+0) << ":" << (2+3+3+3+3+2) << " with lines title 'lleg ee'," - << "'" << fname << "' using " << (tmp_start) << ":" << (tmp_start+2) << " with lines title 'rleg toe'," - << "'" << fname << "' using " << (tmp_start+3) << ":" << (tmp_start+3+2) << " with lines title 'lleg toe'," - << "'" << fname << "' using " << (tmp_start+3+3) << ":" << (tmp_start+3+3+2) << " with lines title 'rleg heel'," - << "'" << fname << "' using " << (tmp_start+3+3+3) << ":" << (tmp_start+3+3+3+2) << " with lines title 'lleg heel'" + << "'" << fname_fpos << "' using " << (2) << ":" << (2+2) << " with lines title 'rleg ee'," + << "'" << fname_fpos << "' using " << (2+3) << ":" << (2+3+2) << " with lines title 'lleg ee'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start) << ":" << (tmp_thpos_start+2) << " with lines title 'rleg toe'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+3) << ":" << (tmp_thpos_start+3+2) << " with lines title 'lleg toe'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3) << ":" << (tmp_thpos_start+3+3+2) << " with lines title 'rleg heel'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3+3) << ":" << (tmp_thpos_start+3+3+3+2) << " with lines title 'lleg heel'" << std::endl; //oss << "set title 'Y-Z'" << std::endl; oss << "set size ratio " << range[2]/range[1] << std::endl; @@ -306,12 +563,12 @@ class testGaitGenerator oss << "plot " << "[" << min_v[1]<< ":" << max_v[1] << "]" << "[" << min_v[2] << ":" << max_v[2] << "]" - << "'" << fname << "' using " << (2+3+3+3+1) << ":" << (2+3+3+3+2) << " with lines title 'rleg ee'," - << "'" << fname << "' using " << (2+3+3+3+3+1) << ":" << (2+3+3+3+3+2) << " with lines title 'lleg ee'," - << "'" << fname << "' using " << (tmp_start+1) << ":" << (tmp_start+2) << " with lines title 'rleg toe'," - << "'" << fname << "' using " << (tmp_start+3+1) << ":" << (tmp_start+3+2) << " with lines title 'lleg toe'," - << "'" << fname << "' using " << (tmp_start+3+3+1) << ":" << (tmp_start+3+3+2) << " with lines title 'rleg heel'," - << "'" << fname << "' using " << (tmp_start+3+3+3+1) << ":" << (tmp_start+3+3+3+2) << " with lines title 'lleg heel'" + << "'" << fname_fpos << "' using " << (2+1) << ":" << (2+2) << " with lines title 'rleg ee'," + << "'" << fname_fpos << "' using " << (2+3+1) << ":" << (2+3+2) << " with lines title 'lleg ee'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+1) << ":" << (tmp_thpos_start+2) << " with lines title 'rleg toe'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+1) << ":" << (tmp_thpos_start+3+2) << " with lines title 'lleg toe'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3+1) << ":" << (tmp_thpos_start+3+3+2) << " with lines title 'rleg heel'," + << "'" << fname_thpos << "' using " << (tmp_thpos_start+3+3+3+1) << ":" << (tmp_thpos_start+3+3+3+2) << " with lines title 'lleg heel'" << std::endl; plot_and_save(gps[6], gtitle, oss.str()); } @@ -326,33 +583,200 @@ class testGaitGenerator << "'" << fname_sstime << "' using 1:" << 2 << " with lines title 'rleg remain time'," << "'" << fname_sstime << "' using 1:" << 3 << " with lines title 'lleg remain time'," << "'" << fname_sstime << "' using 1:" << 4 << " with lines title 'rleg contact states'," - << "'" << fname_sstime << "' using 1:" << 5 << " with lines title 'lleg contact states'" + << "'" << fname_sstime << "' using 1:" << 5 << " with lines title 'lleg contact states'," + << "'" << fname_sstime << "' using 1:" << 6 << " with lines title 'toe_heel_ratio*0.8+0.1'" << std::endl; - plot_and_save(gps[4], gtitle, oss.str()); + plot_and_save(gps[7], gtitle, oss.str()); + } + { + std::ostringstream oss(""); + std::string gtitle("Swing_support_mid_coords_pos"); + size_t tmp_start = 2; + oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; + std::string titles[3] = {"X", "Y", "Z"}; + for (size_t ii = 0; ii < 3; ii++) { + oss << "set xlabel 'Time [s]'" << std::endl; + oss << "set ylabel 'Pos " << titles[ii] << "[m]'" << std::endl; + oss << "plot " + << "'" << fname_ssmc << "' using 1:" << (tmp_start+ii) << " with lines title ''" + << std::endl; + } + plot_and_save(gps[8], gtitle, oss.str()); + } + { + std::ostringstream oss(""); + std::string gtitle("Swing_support_mid_coords_rot"); + size_t tmp_start = 2; + oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; + std::string titles[3] = {"Roll", "Pitch", "Yaw"}; + for (size_t ii = 0; ii < 3; ii++) { + oss << "set xlabel 'Time [s]'" << std::endl; + oss << "set ylabel 'Rot " << titles[ii] << "[deg]'" << std::endl; + oss << "plot " + << "'" << fname_ssmc << "' using 1:" << (tmp_start+ii+3) << " with lines title ''" + << std::endl; + } + plot_and_save(gps[9], gtitle, oss.str()); + } + { + std::ostringstream oss(""); + std::string gtitle("Swing_support_mid_coords_pos_vel"); + size_t tmp_start = 2; + oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; + std::string titles[3] = {"X", "Y", "Z"}; + for (size_t ii = 0; ii < 3; ii++) { + oss << "set xlabel 'Time [s]'" << std::endl; + oss << "set ylabel 'PosVel " << titles[ii] << "[m/s]'" << std::endl; + oss << "plot " + << "'" << fname_ssmcvel << "' using 1:" << (tmp_start+ii) << " with lines title ''" + << std::endl; + } + plot_and_save(gps[10], gtitle, oss.str()); + } + { + std::ostringstream oss(""); + std::string gtitle("Swing_support_mid_coords_rot_vel"); + size_t tmp_start = 2; + oss << "set multiplot layout 3, 1 title '" << gtitle << "'" << std::endl; + std::string titles[3] = {"Roll", "Pitch", "Yaw"}; + for (size_t ii = 0; ii < 3; ii++) { + oss << "set xlabel 'Time [s]'" << std::endl; + oss << "set ylabel 'RotVel " << titles[ii] << "[deg/s]'" << std::endl; + oss << "plot " + << "'" << fname_ssmcvel << "' using 1:" << (tmp_start+ii+3) << " with lines title ''" + << std::endl; + } + plot_and_save(gps[11], gtitle, oss.str()); + } + // + if (use_graph_append) { + int ret = 0; + usleep(500000); // Wait for gnuplot plot finishing (0.5[s]) + ret = system("bash -c 'for f in /tmp/*.eps; do convert -density 250x250 $f ${f//eps}jpg; done'"); + ret = system("(cd /tmp; convert +append Swing_support_mid_coords_pos.jpg Swing_support_mid_coords_pos_vel.jpg Swing_support_mid_coords_rot.jpg Swing_support_mid_coords_rot_vel.jpg img1.jpg)"); + ret = system("(cd /tmp/; convert +append Swing_support_pos.jpg Swing_support_pos_vel.jpg Swing_support_rot.jpg Swing_support_rot_vel.jpg img2.jpg)"); + ret = system("(cd /tmp/; convert +append Swing_support_zmp_offset.jpg Swing_support_remain_time.jpg COG_and_ZMP.jpg Swing_support_pos_trajectory.jpg img3.jpg)"); + std::string tmpstr = test_doc_string.substr(0, test_doc_string.find(":")); + for(size_t c = tmpstr.find_first_of(" "); c != std::string::npos; c = c = tmpstr.find_first_of(" ")){ + tmpstr.erase(c,1); + } + ret = system(std::string("(cd /tmp/; convert -append img1.jpg img2.jpg img3.jpg testGaitGeneratorResults_"+tmpstr+".jpg; rm -f /tmp/img[123].jpg /tmp/COG_and_ZMP.jpg /tmp/Swing_support*.jpg)").c_str()); + } else { + double tmp; + std::cin >> tmp; } - double tmp; - std::cin >> tmp; for (size_t ii = 0; ii < gpsize; ii++) { fprintf(gps[ii], "exit\n"); fflush(gps[ii]); pclose(gps[ii]); } } - std::cerr << "Checking" << std::endl; - std::cerr << " ZMP error : " << is_small_zmp_error << std::endl; - std::cerr << " ZMP diff : " << is_small_zmp_diff << std::endl; - std::cerr << " Contact states & swing support time validity : " << is_contact_states_swing_support_time_validity << std::endl; + std::string tmpstr = test_doc_string.substr(0, test_doc_string.find(":")); + for(size_t c = tmpstr.find_first_of(" "); c != std::string::npos; c = c = tmpstr.find_first_of(" ")){ + tmpstr.erase(c,1); + } + std::cerr << "Checking of " << tmpstr << " : all results = " << (check_all_results()?"true":"false") << std::endl; + std::cerr << " ZMP error : " << (zmp_error_checker.isSmallError()?"true":"false") << ", max_error : " << zmp_error_checker.getMaxValue()*1e3 << "[mm], thre : " << zmp_error_checker.getErrorThre()*1e3 << "[mm]" << std::endl; + std::cerr << " COGZMP error : " << (cogzmp_error_checker.isSmallError()?"true":"false") << ", max_error : " << cogzmp_error_checker.getMaxValue()*1e3 << "[mm], thre : " << cogzmp_error_checker.getErrorThre()*1e3 << "[mm]" << std::endl; + std::cerr << " RefZMP diff : " << (refzmp_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << refzmp_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << refzmp_diff_checker.getDiffThre()*1e3 << "[mm]" << std::endl; + std::cerr << " CartZMP diff : " << (cartzmp_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << cartzmp_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << cartzmp_diff_checker.getDiffThre()*1e3 << "[mm]" << std::endl; + std::cerr << " COG diff : " << (cog_diff_checker.isSmallDiff()?"true":"false") << ", max_diff : " << cog_diff_checker.getMaxValue()*1e3 << "[mm], thre : " << cog_diff_checker.getDiffThre()*1e3 << "[mm]" < neglect_index = boost::assign::list_of(2); + cogzmp_error_checker.checkValueError(gg->get_cog(), gg->get_refzmp(), neglect_index); }; + void check_end_values () + { + // Check if start/end COG(xy) is sufficiently close to REFZMP(xy). + std::vector neglect_index = boost::assign::list_of(2); + cogzmp_error_checker.checkValueError(gg->get_cog(), gg->get_refzmp(), neglect_index); + // Check step times by comparing calculated step time vs input footsteps step times + std::vector< std::vector > fsl; + gg->get_footstep_nodes_list(fsl); + is_step_time_valid = step_time_list.size() == fsl.size(); + if (is_step_time_valid) { + for (size_t i = 0; i < step_time_list.size(); i++) { + is_step_time_valid = eps_eq(step_time_list[i], fsl[i][0].step_time, 1e-5) && is_step_time_valid; + } + } + // Check toe heel angle by comparing calculated toe heel angle (min/max = heel/toe) vs input footsteps toe heel angles + is_toe_heel_dif_angle_valid = (min_toe_heel_dif_angle_list.size() == fsl.size()) && (max_toe_heel_dif_angle_list.size() == fsl.size()); + if (is_toe_heel_dif_angle_valid) { + if (gg->get_use_toe_heel_auto_set()) { + // TODO : not implemented yet + } else { + //for (size_t i = 0; i < min_toe_heel_dif_angle_list.size(); i++) { + for (size_t i = 1; i < min_toe_heel_dif_angle_list.size(); i++) { + // std::cerr << "[" << min_toe_heel_dif_angle_list[i] << " " << max_toe_heel_dif_angle_list[i] << " " + // << -fsl[i][0].heel_angle << " " << fsl[i][0].toe_angle << "]" << std::endl; + is_toe_heel_dif_angle_valid = eps_eq(min_toe_heel_dif_angle_list[i], (-fsl[i][0].heel_angle), 1e-5) + && eps_eq(max_toe_heel_dif_angle_list[i], fsl[i][0].toe_angle, 1e-5) + && is_toe_heel_dif_angle_valid; + } + } + } + // Check validity of toe heel + zmp offset + // If use_toe_heel_transition is true, use/not-use of toe/heel angle corresponds to use/not-use zmp transition of toe/heel. + is_toe_heel_zmp_offset_x_valid = (min_zmp_offset_x_list.size() == fsl.size()) && (max_zmp_offset_x_list.size() == fsl.size()); + if (gg->get_use_toe_heel_transition()) { + for (size_t i = 0; i < min_zmp_offset_x_list.size(); i++) { + // std::cerr << "[" << min_zmp_offset_x_list[i] << " " << max_zmp_offset_x_list[i] << " " << gg->get_toe_zmp_offset_x() << " " << gg->get_heel_zmp_offset_x() << "]" << std::endl; + bool heel_valid = true; + if ( !eps_eq(min_toe_heel_dif_angle_list[i], 0.0, 1e-5) ) { // If use heel, zmp offset should be same as heel zmp offset + heel_valid = eps_eq(min_zmp_offset_x_list[i], gg->get_heel_zmp_offset_x(), 1e-5); + } else { // If not use heel, zmp offset should not be same as heel zmp offset + heel_valid = !eps_eq(min_zmp_offset_x_list[i], gg->get_heel_zmp_offset_x(), 1e-5); + } + bool toe_valid = true; + if ( !eps_eq(max_toe_heel_dif_angle_list[i], 0.0, 1e-5) ) { // If use toe, zmp offset should be same as toe zmp offset + toe_valid = eps_eq(max_zmp_offset_x_list[i], gg->get_toe_zmp_offset_x(), 1e-5); + } else { // If not use toe, zmp offset should not be same as toe zmp offset + toe_valid = !eps_eq(max_zmp_offset_x_list[i], gg->get_toe_zmp_offset_x(), 1e-5); + } + // std::cerr << heel_valid << " " << toe_valid << std::endl; + is_toe_heel_zmp_offset_x_valid = heel_valid && toe_valid && is_toe_heel_zmp_offset_x_valid; + } + } else { + // TODO : not implemented yet + } + }; + + // Generate and plot walk pattern void gen_and_plot_walk_pattern(const step_node& initial_support_leg_step, const step_node& initial_swing_leg_dst_step) { - parse_params(); + parse_params(false); gg->print_param(); - gg->initialize_gait_parameter(cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step)); + gg->initialize_gait_parameter(cog, cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step)); while ( !gg->proc_one_tick() ); //gg->print_footstep_list(); - plot_walk_pattern(); - } + /* make step and dump */ + check_start_values(); + size_t i = 0; + while ( gg->proc_one_tick() ) { + proc_one_walking_motion(i); + i++; + } + check_end_values(); + plot_and_print_errorcheck (); + }; void gen_and_plot_walk_pattern() { @@ -370,18 +794,51 @@ class testGaitGenerator public: std::vector arg_strs; - testGaitGenerator() : use_gnuplot(true), is_small_zmp_error(true), is_small_zmp_diff(true), is_contact_states_swing_support_time_validity(true) {}; + testGaitGenerator(double _dt) : dt(_dt), use_gnuplot(true), use_graph_append(false), is_contact_states_swing_support_time_validity(true), + refzmp_diff_checker(20.0*1e-3), cartzmp_diff_checker(20.0*1e-3), cog_diff_checker(10.0*1e-3), // [mm] + ssmcpos_diff_checker(10.0*1e-3), ssmcrot_diff_checker(deg2rad(0.1)), + ssmcposvel_diff_checker(10.0*1e-3), ssmcrotvel_diff_checker(deg2rad(1)), + footpos_diff_checker(10.0*1e-3), footrot_diff_checker(deg2rad(1)), + footposvel_diff_checker(40*1e-3), footrotvel_diff_checker(deg2rad(15)), + zmpoffset_diff_checker(20.0*1e-3), + zmp_error_checker(50*1e-3), cogzmp_error_checker(1.5*1e-3), + min_rfoot_pos(1e10,1e10,1e10), min_lfoot_pos(1e10,1e10,1e10), max_rfoot_pos(-1e10,-1e10,-1e10), max_lfoot_pos(-1e10,-1e10,-1e10), + min_toe_heel_dif_angle(1e10), max_toe_heel_dif_angle(-1e10), min_zmp_offset_x(1e10), max_zmp_offset_x(-1e10), + prev_contact_states(2, true), // RLEG, LLEG + prev_swing_support_time(2, 1e2), // RLEG, LLEG + fname_cogzmp("/tmp/plot-cogzmp.dat"), fp_cogzmp(fopen(fname_cogzmp.c_str(), "w")), + fname_fpos("/tmp/plot-fpos.dat"), fp_fpos(fopen(fname_fpos.c_str(), "w")), + fname_frot("/tmp/plot-frot.dat"), fp_frot(fopen(fname_frot.c_str(), "w")), + fname_zoff("/tmp/plot-zoff.dat"), fp_zoff(fopen(fname_zoff.c_str(), "w")), + fname_fposvel("/tmp/plot-fposvel.dat"), fp_fposvel(fopen(fname_fposvel.c_str(), "w")), + fname_frotvel("/tmp/plot-frotvel.dat"), fp_frotvel(fopen(fname_frotvel.c_str(), "w")), + fname_thpos("/tmp/plot-thpos.dat"), fp_thpos(fopen(fname_thpos.c_str(), "w")), + fname_sstime("/tmp/plot-sstime.dat"), fp_sstime(fopen(fname_sstime.c_str(), "w")), + fname_ssmc("/tmp/plot-ssmc.dat"), fp_ssmc(fopen(fname_ssmc.c_str(), "w")), + fname_ssmcvel("/tmp/plot-ssmcvel.dat"), fp_ssmcvel(fopen(fname_ssmcvel.c_str(), "w")) + {}; + virtual ~testGaitGenerator() { if (gg != NULL) { delete gg; gg = NULL; } + fclose(fp_cogzmp); + fclose(fp_fpos); + fclose(fp_frot); + fclose(fp_zoff); + fclose(fp_fposvel); + fclose(fp_frotvel); + fclose(fp_thpos); + fclose(fp_sstime); + fclose(fp_ssmc); + fclose(fp_ssmcvel); }; void test0 () { - std::cerr << "test0 : Set foot steps" << std::endl; + test_doc_string = "test0 : Set foot steps"; /* initialize sample footstep_list */ parse_params(); std::vector< std::vector > fnsl; @@ -396,7 +853,7 @@ class testGaitGenerator void test1 () { - std::cerr << "test1 : Go pos x,y,th combination" << std::endl; + test_doc_string = "test1 : Go pos x,y,th combination"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -408,7 +865,7 @@ class testGaitGenerator void test2 () { - std::cerr << "test2 : Go pos x" << std::endl; + test_doc_string = "test2 : Go pos x"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -420,7 +877,7 @@ class testGaitGenerator void test3 () { - std::cerr << "test3 : Go pos y" << std::endl; + test_doc_string = "test3 : Go pos y"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -432,7 +889,7 @@ class testGaitGenerator void test4 () { - std::cerr << "test4 : Go pos th" << std::endl; + test_doc_string = "test4 : Go pos th"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -444,9 +901,10 @@ class testGaitGenerator void test5 () { - std::cerr << "test5 : Set foot steps with Z change" << std::endl; + test_doc_string = "test5 : Set foot steps with Z change"; /* initialize sample footstep_list */ parse_params(); + gg->set_default_orbit_type(CYCLOIDDELAY); std::vector< std::vector > fnsl; fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); @@ -460,16 +918,20 @@ class testGaitGenerator void test6 () { - std::cerr << "test6 : Go single step" << std::endl; + test_doc_string = "test6 : Go single step"; parse_params(); gg->clear_footstep_nodes_list(); - gg->go_single_step_param_2_footstep_nodes_list(100*1e-3, 0, 0, 0, "rleg", coordinates(leg_pos[0])); + std::vector< std::vector > fnsl; + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 75*1e-3, 0)+leg_pos[1]), hrp::rotFromRpy(hrp::Vector3(deg2rad(5), deg2rad(-20), deg2rad(10)))), + gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + gg->set_foot_steps_list(fnsl); gen_and_plot_walk_pattern(); }; void test7 () { - std::cerr << "test7 : Toe heel walk" << std::endl; + test_doc_string = "test7 : Toe heel walk"; /* initialize sample footstep_list */ parse_params(); std::vector dzo; @@ -480,6 +942,8 @@ class testGaitGenerator gg->set_heel_zmp_offset_x(-105*1e-3); gg->set_toe_pos_offset_x(137*1e-3); gg->set_heel_pos_offset_x(-105*1e-3); + gg->set_stride_parameters(0.2,0.1,20,0.2,0.1*0.5,20*0.5); + gg->set_use_toe_heel_auto_set(true); gg->set_toe_angle(30); gg->set_heel_angle(10); // gg->set_use_toe_heel_transition(false); @@ -487,13 +951,13 @@ class testGaitGenerator gg->clear_footstep_nodes_list(); coordinates start_ref_coords; mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0])); - gg->go_pos_param_2_footstep_nodes_list(100*1e-3, 0, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG)); + gg->go_pos_param_2_footstep_nodes_list(400*1e-3, 0, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG)); gen_and_plot_walk_pattern(); }; void test8 () { - std::cerr << "test8 : Toe heel walk on slope" << std::endl; + test_doc_string = "test8 : Toe heel walk on slope"; /* initialize sample footstep_list */ parse_params(); std::vector dzo; @@ -529,7 +993,7 @@ class testGaitGenerator void test9 () { - std::cerr << "test9 : Stair walk" << std::endl; + test_doc_string = "test9 : Stair walk"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -550,7 +1014,7 @@ class testGaitGenerator void test10 () { - std::cerr << "test10 : Stair walk + toe heel contact" << std::endl; + test_doc_string = "test10 : Stair walk + toe heel contact"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -583,7 +1047,7 @@ class testGaitGenerator void test11 () { - std::cerr << "test11 : Foot rot change" << std::endl; + test_doc_string = "test11 : Foot rot change"; /* initialize sample footstep_list */ parse_params(); hrp::Matrix33 tmpr; @@ -599,7 +1063,7 @@ class testGaitGenerator void test12 () { - std::cerr << "test12 : Change step param in set foot steps" << std::endl; + test_doc_string = "test12 : Change step param in set foot steps"; /* initialize sample footstep_list */ parse_params(); std::vector< std::vector > fnsl; @@ -616,7 +1080,7 @@ class testGaitGenerator void test13 () { - std::cerr << "test13 : Arbitrary leg switching" << std::endl; + test_doc_string = "test13 : Arbitrary leg switching"; /* initialize sample footstep_list */ parse_params(); std::vector< std::vector > fnsl; @@ -632,7 +1096,7 @@ class testGaitGenerator void test14 () { - std::cerr << "test14 : kick walk" << std::endl; + test_doc_string = "test14 : kick walk"; /* initialize sample footstep_list */ parse_params(); gg->clear_footstep_nodes_list(); @@ -643,10 +1107,165 @@ class testGaitGenerator gen_and_plot_walk_pattern(); }; + void test15 () + { + test_doc_string = "test15 : Stair walk down"; + /* initialize sample footstep_list */ + parse_params(); + gg->clear_footstep_nodes_list(); + gg->set_default_orbit_type(STAIR); + gg->set_swing_trajectory_delay_time_offset (0.2); + std::vector< std::vector > fnsl; + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, -200*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(250*1e-3, 0, -200*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, -400*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(500*1e-3, 0, -400*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, -600*1e-3)+leg_pos[0])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(750*1e-3, 0, -600*1e-3)+leg_pos[1])), gg->get_default_step_height(), gg->get_default_step_time(), gg->get_toe_angle(), gg->get_heel_angle()))); + gg->set_foot_steps_list(fnsl); + gen_and_plot_walk_pattern(); + }; - void parse_params () + void test16 () { - for (int i = 0; i < arg_strs.size(); ++ i) { + test_doc_string = "test16 : Set foot steps with param (toe heel contact)"; + /* initialize sample footstep_list */ + parse_params(); + gg->clear_footstep_nodes_list(); + std::vector dzo; + dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0)); + dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0)); + gg->set_default_zmp_offsets(dzo); + gg->set_toe_zmp_offset_x(137*1e-3); + gg->set_heel_zmp_offset_x(-105*1e-3); + gg->set_toe_pos_offset_x(137*1e-3); + gg->set_heel_pos_offset_x(-105*1e-3); + gg->set_toe_angle(20); + gg->set_heel_angle(5); + gg->set_default_step_time(2); + gg->set_default_double_support_ratio_before(0.1); + gg->set_default_double_support_ratio_after(0.1); + gg->set_use_toe_heel_auto_set(true); + gg->set_use_toe_heel_transition(true); + //double ratio[7] = {0.02, 0.28, 0.2, 0.0, 0.2, 0.25, 0.05}; + double ratio[7] = {0.07, 0.20, 0.2, 0.0, 0.2, 0.25, 0.08}; + std::vector ratio2(ratio, ratio+gg->get_NUM_TH_PHASES()); + gg->set_toe_heel_phase_ratio(ratio2); + std::vector< std::vector > fnsl; + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(0, 0, 0)+leg_pos[0])), gg->get_default_step_height()*0.5, gg->get_default_step_time()*0.5, gg->get_toe_angle()*0.5, gg->get_heel_angle()*0.5))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(100*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height()*2.0, gg->get_default_step_time()*2.0, gg->get_toe_angle()*2.0, gg->get_heel_angle()*2.0))); + fnsl.push_back(boost::assign::list_of(step_node("rleg", coordinates(hrp::Vector3(hrp::Vector3(400*1e-3, 0, 0)+leg_pos[0])), gg->get_default_step_height()*0.5, gg->get_default_step_time()*0.5, gg->get_toe_angle()*0.5, gg->get_heel_angle()*0.5))); + fnsl.push_back(boost::assign::list_of(step_node("lleg", coordinates(hrp::Vector3(hrp::Vector3(400*1e-3, 0, 0)+leg_pos[1])), gg->get_default_step_height()*2.0, gg->get_default_step_time()*2.0, gg->get_toe_angle()*2.0, gg->get_heel_angle()*2.0))); + gg->set_foot_steps_list(fnsl); + gen_and_plot_walk_pattern(); + }; + + void test17 () + { + test_doc_string = "test17 : Test goVelocity (dx = 0.1, dy = 0.05, dth = 10.0)"; + /* initialize sample footstep_list */ + parse_params(); + gg->clear_footstep_nodes_list(); + gg->print_param(); + step_node initial_support_leg_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0); + step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0); + coordinates fm_coords; + mid_coords(fm_coords, 0.5, initial_swing_leg_dst_step.worldcoords, initial_support_leg_step.worldcoords); + gg->initialize_velocity_mode(fm_coords, 0.1, 0.05, 10.0, boost::assign::list_of(RLEG)); + gg->initialize_gait_parameter(cog, cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step)); + while ( !gg->proc_one_tick() ); + /* make step and dump */ + check_start_values(); + size_t i = 0; + while ( gg->proc_one_tick() ) { + proc_one_walking_motion(i); + i++; + if ( i > static_cast(gg->get_default_step_time()/dt) && gg->get_overwrite_check_timing() ) { + gg->finalize_velocity_mode(); + } + } + check_end_values(); + plot_and_print_errorcheck (); + }; + + void test18 () + { + test_doc_string = "test18 : Test goVelocity with changing velocity (translation and rotation)"; + /* initialize sample footstep_list */ + parse_params(); + gg->clear_footstep_nodes_list(); + gg->set_overwritable_footstep_index_offset(0); + gg->set_default_orbit_type(CYCLOIDDELAY); + gg->print_param(); + step_node initial_support_leg_step = step_node(LLEG, coordinates(leg_pos[1]), 0, 0, 0, 0); + step_node initial_swing_leg_dst_step = step_node(RLEG, coordinates(leg_pos[0]), 0, 0, 0, 0); + coordinates fm_coords; + mid_coords(fm_coords, 0.5, initial_swing_leg_dst_step.worldcoords, initial_support_leg_step.worldcoords); + gg->initialize_velocity_mode(fm_coords, 0, 0, 0, boost::assign::list_of(RLEG)); + gg->initialize_gait_parameter(cog, cog, boost::assign::list_of(initial_support_leg_step), boost::assign::list_of(initial_swing_leg_dst_step)); + while ( !gg->proc_one_tick() ); + /* make step and dump */ + check_start_values(); + size_t i = 0; + while ( gg->proc_one_tick() ) { + proc_one_walking_motion(i); + i++; + if ( i > static_cast(gg->get_default_step_time()/dt)*5 && gg->get_overwrite_check_timing() ) { + gg->finalize_velocity_mode(); + } else if ( i > static_cast(gg->get_default_step_time()/dt)*4 && gg->get_overwrite_check_timing() ) { + gg->set_velocity_param(0, 0, 0); + } else if ( i > static_cast(gg->get_default_step_time()/dt)*3 && gg->get_overwrite_check_timing() ) { + gg->set_velocity_param(0, 0, 10); + } else if ( i > static_cast(gg->get_default_step_time()/dt)*2 && gg->get_overwrite_check_timing() ) { + gg->set_velocity_param(0, 0, 0); + } else if ( i > static_cast(gg->get_default_step_time()/dt) && gg->get_overwrite_check_timing() ) { + gg->set_velocity_param(0.1, 0.05, 0); + } + } + check_end_values(); + plot_and_print_errorcheck (); + }; + + void test19 () + { + test_doc_string = "test19 : Change stride parameter (translate)"; + /* initialize sample footstep_list */ + parse_params(); + std::vector dzo; + dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0)); + dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0)); + gg->set_default_zmp_offsets(dzo); + gg->set_stride_parameters(0.25,0.15,25,0.25,0.1,10); + gg->clear_footstep_nodes_list(); + coordinates start_ref_coords; + mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0])); + gg->go_pos_param_2_footstep_nodes_list(600*1e-3, -300*1e-3, 0, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG)); + gen_and_plot_walk_pattern(); + }; + + void test20 () + { + test_doc_string = "test20 : Change stride parameter (translate+rotate)"; + /* initialize sample footstep_list */ + parse_params(); + std::vector dzo; + dzo.push_back(hrp::Vector3(20*1e-3,-30*1e-3,0)); + dzo.push_back(hrp::Vector3(20*1e-3,30*1e-3,0)); + gg->set_default_zmp_offsets(dzo); + gg->set_stride_parameters(0.25,0.15,25,0.25,0.1,10); + gg->clear_footstep_nodes_list(); + coordinates start_ref_coords; + mid_coords(start_ref_coords, 0.5, coordinates(leg_pos[1]), coordinates(leg_pos[0])); + gg->go_pos_param_2_footstep_nodes_list(400*1e-3, -200*1e-3, -55, boost::assign::list_of(coordinates(leg_pos[1])), start_ref_coords, boost::assign::list_of(LLEG)); + gen_and_plot_walk_pattern(); + }; + + void parse_params (bool is_print_doc_setring = true) + { + if (is_print_doc_setring) std::cerr << test_doc_string << std::endl; + for (unsigned int i = 0; i < arg_strs.size(); ++ i) { if ( arg_strs[i]== "--default-step-time" ) { if (++i < arg_strs.size()) gg->set_default_step_time(atof(arg_strs[i].c_str())); } else if ( arg_strs[i]== "--default-step-height" ) { @@ -679,10 +1298,16 @@ class testGaitGenerator if (++i < arg_strs.size()) gg->set_default_double_support_static_ratio_before(atof(arg_strs[i].c_str())); } else if ( arg_strs[i]== "--default-double-support-static-ratio-after" ) { if (++i < arg_strs.size()) gg->set_default_double_support_static_ratio_after(atof(arg_strs[i].c_str())); + } else if ( arg_strs[i]== "--default-double-support-ratio-swing-before" ) { + if (++i < arg_strs.size()) gg->set_default_double_support_ratio_swing_before(atof(arg_strs[i].c_str())); + } else if ( arg_strs[i]== "--default-double-support-ratio-swing-after" ) { + if (++i < arg_strs.size()) gg->set_default_double_support_ratio_swing_after(atof(arg_strs[i].c_str())); } else if ( arg_strs[i]== "--swing-trajectory-delay-time-offset" ) { if (++i < arg_strs.size()) gg->set_swing_trajectory_delay_time_offset(atof(arg_strs[i].c_str())); } else if ( arg_strs[i]== "--swing-trajectory-final-distance-weight" ) { if (++i < arg_strs.size()) gg->set_swing_trajectory_final_distance_weight(atof(arg_strs[i].c_str())); + } else if ( arg_strs[i]== "--swing-trajectory-time-offset-xy2z" ) { + if (++i < arg_strs.size()) gg->set_swing_trajectory_time_offset_xy2z(atof(arg_strs[i].c_str())); } else if ( arg_strs[i]== "--stair-trajectory-way-point-offset" ) { if (++i < arg_strs.size()) { coil::vstring strs = coil::split(std::string(arg_strs[i].c_str()), ","); @@ -711,35 +1336,44 @@ class testGaitGenerator if (++i < arg_strs.size()) gg->set_optional_go_pos_finalize_footstep_num(atoi(arg_strs[i].c_str())); } else if ( arg_strs[i]== "--use-gnuplot" ) { if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true"); + } else if ( arg_strs[i]== "--use-graph-append" ) { + if (++i < arg_strs.size()) use_graph_append = (arg_strs[i]=="true"); } } }; - bool check_all_results () + bool check_all_results () const { - return is_small_zmp_error && is_small_zmp_diff && is_contact_states_swing_support_time_validity; + return refzmp_diff_checker.isSmallDiff() && cartzmp_diff_checker.isSmallDiff() && cog_diff_checker.isSmallDiff() + && ssmcpos_diff_checker.isSmallDiff() && ssmcrot_diff_checker.isSmallDiff() + && ssmcposvel_diff_checker.isSmallDiff() && ssmcrotvel_diff_checker.isSmallDiff() + && footpos_diff_checker.isSmallDiff() && footrot_diff_checker.isSmallDiff() + && zmpoffset_diff_checker.isSmallDiff() + && footposvel_diff_checker.isSmallDiff() && footrotvel_diff_checker.isSmallDiff() + && zmp_error_checker.isSmallError() && cogzmp_error_checker.isSmallError() + && is_step_time_valid && is_toe_heel_dif_angle_valid && is_toe_heel_zmp_offset_x_valid + && is_contact_states_swing_support_time_validity; }; }; class testGaitGeneratorHRP2JSK : public testGaitGenerator { public: - testGaitGeneratorHRP2JSK () + testGaitGeneratorHRP2JSK () : testGaitGenerator(0.004) { - dt = 0.004; cog = 1e-3*hrp::Vector3(6.785, 1.54359, 806.831); leg_pos.push_back(hrp::Vector3(0,1e-3*-105,0)); /* rleg */ leg_pos.push_back(hrp::Vector3(0,1e-3* 105,0)); /* lleg */ all_limbs.push_back("rleg"); all_limbs.push_back("lleg"); - gg = new gait_generator(dt, leg_pos, all_limbs, 1e-3*150, 1e-3*50, 10, 1e-3*50); + gg = new gait_generator(dt, leg_pos, all_limbs, 1e-3*150, 1e-3*50, 10, 1e-3*50, 1e-3*50*0.5, 10*0.5); }; }; void print_usage () { - std::cerr << "Usage : testGaitGenerator [option]" << std::endl; - std::cerr << " [option] should be:" << std::endl; + std::cerr << "Usage : testGaitGenerator [test-name] [option]" << std::endl; + std::cerr << " [test-name] should be:" << std::endl; std::cerr << " --test0 : Set foot steps" << std::endl; std::cerr << " --test1 : Go pos x,y,th combination" << std::endl; std::cerr << " --test2 : Go pos x" << std::endl; @@ -751,11 +1385,20 @@ void print_usage () std::cerr << " --test8 : Toe heel walk on slope" << std::endl; std::cerr << " --test9 : Stair walk" << std::endl; std::cerr << " --test10 : Stair walk + toe heel contact" << std::endl; - std::cerr << " --test10 : Stair walk + toe heel contact" << std::endl; std::cerr << " --test11 : Foot rot change" << std::endl; std::cerr << " --test12 : Change step param in set foot steps" << std::endl; std::cerr << " --test13 : Arbitrary leg switching" << std::endl; std::cerr << " --test14 : kick walk" << std::endl; + std::cerr << " --test15 : Stair walk down" << std::endl; + std::cerr << " --test16 : Set foot steps with param (toe heel contact)" << std::endl; + std::cerr << " --test17 : Test goVelocity (dx = 0.1, dy = 0.05, dth = 10.0)" << std::endl; + std::cerr << " --test18 : Test goVelocity with changing velocity (translation and rotation)" << std::endl; + std::cerr << " --test19 : Change stride parameter (translate)" << std::endl; + std::cerr << " --test20 : Change stride parameter (translate+rotate)" << std::endl; + std::cerr << " [option] should be:" << std::endl; + std::cerr << " --use-gnuplot : Use gnuplot and dump eps file to /tmp. (true/false, true by default)" << std::endl; + std::cerr << " --use-graph-append : Append generated graph to /tmp/testGaitGenerator.jpg. (true/false, false by default)" << std::endl; + std::cerr << " other options : for GaitGenerator" << std::endl; }; int main(int argc, char* argv[]) @@ -796,6 +1439,18 @@ int main(int argc, char* argv[]) tgg.test13(); } else if (std::string(argv[1]) == "--test14") { tgg.test14(); + } else if (std::string(argv[1]) == "--test15") { + tgg.test15(); + } else if (std::string(argv[1]) == "--test16") { + tgg.test16(); + } else if (std::string(argv[1]) == "--test17") { + tgg.test17(); + } else if (std::string(argv[1]) == "--test18") { + tgg.test18(); + } else if (std::string(argv[1]) == "--test19") { + tgg.test19(); + } else if (std::string(argv[1]) == "--test20") { + tgg.test20(); } else { print_usage(); ret = 1; diff --git a/rtc/AverageFilter/AverageFilter.cpp b/rtc/AverageFilter/AverageFilter.cpp index e2445822382..993f315d1d3 100644 --- a/rtc/AverageFilter/AverageFilter.cpp +++ b/rtc/AverageFilter/AverageFilter.cpp @@ -10,7 +10,7 @@ #include #include #include "AverageFilter.h" -#include "pointcloud.hh" +#include "hrpsys/idl/pointcloud.hh" // Module specification // @@ -149,7 +149,7 @@ RTC::ReturnCode_t AverageFilter::onExecute(RTC::UniqueId ec_id) // compute bbox float xmin, xmax, ymin, ymax; float *src = (float *)m_original.data.get_buffer(); - int npoint = m_original.data.length()/m_original.point_step; + unsigned int npoint = m_original.data.length()/m_original.point_step; for (unsigned int i=0; i +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include "pointcloud.hh" // Service implementation headers // diff --git a/rtc/AverageFilter/CMakeLists.txt b/rtc/AverageFilter/CMakeLists.txt index 1333e54e3f0..3cad53cbcee 100644 --- a/rtc/AverageFilter/CMakeLists.txt +++ b/rtc/AverageFilter/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(AverageFilterComp ${libs}) set(target AverageFilter AverageFilterComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Beeper/Beeper.cpp b/rtc/Beeper/Beeper.cpp new file mode 100644 index 00000000000..0249d6e7fa1 --- /dev/null +++ b/rtc/Beeper/Beeper.cpp @@ -0,0 +1,257 @@ +// -*- C++ -*- +/*! + * @file Beeper.cpp + * @brief Beeper component + * $Date$ + * + * $Id$ + */ + +#include "Beeper.h" +#include +#include + +// Variables for beep_thread +struct BeepData +{ + bool _beep_start; + int _beep_freq; + int _beep_length; +}; + +bool is_initialized = false; +pthread_mutex_t beep_mutex; // Mutex +double m_dt = 0.002; +std::deque beep_command_buffer; // Used for communication between beepthread and real-time thread + +// Module specification +// +static const char* beeper_spec[] = + { + "implementation_id", "Beeper", + "type_name", "Beeper", + "description", "beeper", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.debugLevel", "0", + "" + }; +// + +void* call_beep (void* args) +{ + // Initialize + init_beep(); + bool wait_for_initialized = true; + while (wait_for_initialized) { // Wait until m_dt is set + usleep(2000); + pthread_mutex_lock( &beep_mutex ); + wait_for_initialized = !is_initialized; + pthread_mutex_unlock( &beep_mutex ); + } + // Loop + bool prev_beep_start=false; + int prev_beep_length = 1000; + while (1) { + // Get beepCommand from buffer + pthread_mutex_lock( &beep_mutex ); + BeepData bd = beep_command_buffer.front(); + if (beep_command_buffer.size() > 1) beep_command_buffer.pop_front(); + pthread_mutex_unlock( &beep_mutex ); +// if (!prev_beep_start && tmp_beep_start) { +// std::cerr << "BP START" << std::endl; +// } else if (prev_beep_start && !tmp_beep_start) { +// std::cerr << "BP STOP" << std::endl; +// } + // Beep + if (bd._beep_start) { + usleep(std::max(static_cast(prev_beep_length*1000),0)); + start_beep(bd._beep_freq, bd._beep_length); + } else if (prev_beep_start) { // If !beep_start and prev_beep_start, stop_beep just once. + usleep(static_cast(1000000*m_dt)); + stop_beep(); + } else { + usleep(static_cast(1000000*m_dt)); + } + prev_beep_start = bd._beep_start; + prev_beep_length = bd._beep_length; + } + quit_beep(); +} + +Beeper::Beeper(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_beepCommandIn("beepCommand", m_beepCommand), + m_loop(0), + m_debugLevel(0) +{ + pthread_create(&beep_thread, NULL, call_beep, (void*)NULL); + pthread_mutex_init( &beep_mutex, NULL ); +} + +Beeper::~Beeper() +{ + pthread_join(beep_thread, NULL ); +} + +RTC::ReturnCode_t Beeper::onInitialize() +{ + std::cerr << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("debugLevel", m_debugLevel, "0"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("beepCommand", m_beepCommandIn); + + // Set OutPort buffer + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + RTC::Properties& prop = getProperties(); + coil::stringTo(m_dt, prop["dt"].c_str()); + pthread_mutex_lock( &beep_mutex ); + is_initialized = true; + pthread_mutex_unlock( &beep_mutex ); + std::cerr << "[" << m_profile.instance_name << "] : Beep thread dt = " << m_dt << "[s]" << std::endl; + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t Beeper::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t Beeper::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t Beeper::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t Beeper::onActivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name << "] : onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t Beeper::onDeactivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name << "] : onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +#define DEBUGP ((m_debugLevel==1 && m_loop%200==0) || m_debugLevel > 1 ) +RTC::ReturnCode_t Beeper::onExecute(RTC::UniqueId ec_id) +{ + // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << "), data = " << m_data.data << std::endl; + m_loop++; + + if (m_beepCommandIn.isNew()) { + // Read beepCommand from data port + m_beepCommandIn.read(); + BeepData bd; + bd._beep_start = (m_beepCommand.data[BEEP_INFO_START] == 1); + bd._beep_freq = m_beepCommand.data[BEEP_INFO_FREQ]; + bd._beep_length = m_beepCommand.data[BEEP_INFO_LENGTH]; + // Push beepCommand to buffer + size_t max_buffer_length = 10; + if (pthread_mutex_trylock( &beep_mutex ) == 0) { + beep_command_buffer.push_back(bd); + while (beep_command_buffer.size() > max_buffer_length) beep_command_buffer.pop_front(); + pthread_mutex_unlock( &beep_mutex ); + } else { + std::cerr << "[" << m_profile.instance_name<< "] Mutex trylock failed (loop=" << m_loop << ")" << std::endl; + } + // print + if (m_debugLevel > 0) { + if (bd._beep_start) + std::cerr << "[" << m_profile.instance_name<< "] isNew : beep start (freq=" << bd._beep_freq << ", length=" << bd._beep_length << ", loop=" << m_loop << ")" << std::endl; + else + std::cerr << "[" << m_profile.instance_name<< "] isNew : beep stop (loop=" << m_loop << ")" << std::endl; + } + } +// if (beep_start) { +// start_beep(beep_freq, beep_length, true); +// } else { +// stop_beep(true); +// } + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t Beeper::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t Beeper::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t Beeper::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t Beeper::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t Beeper::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + + +extern "C" +{ + + void BeeperInit(RTC::Manager* manager) + { + RTC::Properties profile(beeper_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/Beeper/Beeper.h b/rtc/Beeper/Beeper.h new file mode 100644 index 00000000000..1e35b929a35 --- /dev/null +++ b/rtc/Beeper/Beeper.h @@ -0,0 +1,144 @@ +// -*- C++ -*- +/*! + * @file Beeper.h + * @brief Beeper component + * @date $Date$ + * + * $Id$ + */ + +#ifndef BEEPER_H +#define BEEPER_H + +#include +#include +#include +#include +#include +#include +#include +#include "../SoftErrorLimiter/beep.h" + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief sample RT component which has one data input port and one data output port + */ +class Beeper + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + Beeper(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~Beeper(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + protected: + // Configuration variable declaration + // + + // + TimedLongSeq m_beepCommand; + + // DataInPort declaration + // + InPort m_beepCommandIn; + + // + + // DataOutPort declaration + // + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + long long m_loop; + unsigned int m_debugLevel; + pthread_t beep_thread; +}; + + +extern "C" +{ + void BeeperInit(RTC::Manager* manager); +}; + +#endif // BEEPER_H diff --git a/rtc/Beeper/Beeper.txt b/rtc/Beeper/Beeper.txt new file mode 100644 index 00000000000..29289e15f26 --- /dev/null +++ b/rtc/Beeper/Beeper.txt @@ -0,0 +1,53 @@ +/** + +\page Beeper + +\section introduction Overview + +This component is a RT component to beep sound using /dev/console. +Because we use non-real-time-thread for beeping, beeping is not real time. +Beep commands come from several RTCs and the latter RTC has high priority. + + + + +
    implementation_idBeeper
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + +
    port namedata typeunitdescription
    beepCommandRTC::TimedLongSeqBeep Command (please see beep.h)
    + +\subsection outports Output Ports + + + +
    port namedata typeunitdescription
    + +\section serviceports Service Ports + +\subsection provider Service Providers + + + +
    port nameinterface nameservice typeIDLdescription
    + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + +
    nametypeunitdefault valuedescription
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/Beeper/BeeperComp.cpp b/rtc/Beeper/BeeperComp.cpp new file mode 100644 index 00000000000..0ef8e863662 --- /dev/null +++ b/rtc/Beeper/BeeperComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file BeeperComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "Beeper.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + BeeperInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("Beeper"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/Beeper/CMakeLists.txt b/rtc/Beeper/CMakeLists.txt new file mode 100644 index 00000000000..13338ff6108 --- /dev/null +++ b/rtc/Beeper/CMakeLists.txt @@ -0,0 +1,16 @@ +#set(comp_sources Beeper.cpp BeeperService_impl.cpp ../SoftErrorLimiter/beep.cpp) +set(comp_sources Beeper.cpp ../SoftErrorLimiter/beep.cpp) +set(libs hrpModel-3.1 hrpUtil-3.1 hrpsysBaseStub) +add_library(Beeper SHARED ${comp_sources}) +target_link_libraries(Beeper ${libs}) +set_target_properties(Beeper PROPERTIES PREFIX "") + +add_executable(BeeperComp BeeperComp.cpp ${comp_sources}) +target_link_libraries(BeeperComp ${libs}) + +set(target Beeper BeeperComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/CMakeLists.txt b/rtc/CMakeLists.txt index dbd6f7a4cdd..a292b3849f6 100644 --- a/rtc/CMakeLists.txt +++ b/rtc/CMakeLists.txt @@ -36,6 +36,7 @@ add_subdirectory(EmergencyStopper) if (USE_HRPSYSUTIL) add_subdirectory(Viewer) add_subdirectory(CameraImageViewer) + add_subdirectory(ColorExtractor) add_subdirectory(JpegDecoder) add_subdirectory(JpegEncoder) add_subdirectory(RGB2Gray) @@ -46,11 +47,17 @@ if (USE_HRPSYSUTIL) add_subdirectory(RangeDataViewer) add_subdirectory(UndistortImage) add_subdirectory(CameraImageLoader) + add_subdirectory(CameraImageSaver) endif() if (QHULL_FOUND) add_subdirectory(CollisionDetector) endif() add_subdirectory(PDcontroller) +add_subdirectory(ModifiedServo) +add_subdirectory(Beeper) +add_subdirectory(ReferenceForceUpdater) +add_subdirectory(AccelerationFilter) +add_subdirectory(ObjectContactTurnaroundDetector) if (NOT APPLE AND USE_HRPSYSUTIL) add_subdirectory(VideoCapture) @@ -67,13 +74,21 @@ if (OCTOMAP_FOUND) add_subdirectory(OccupancyGridMap3D) endif() -find_package(PCL) +if (NOT "${PCL_LIBRARIES}" STREQUAL "") + list(REMOVE_ITEM PCL_LIBRARIES "vtkproj4") +endif() + if (PCL_FOUND) + add_subdirectory(ApproximateVoxelGridFilter) add_subdirectory(SORFilter) add_subdirectory(MLSFilter) add_subdirectory(PCDLoader) add_subdirectory(PlaneRemover) + add_subdirectory(PointCloudViewer) add_subdirectory(VoxelGridFilter) + if (OPENNI2_FOUND AND EXISTS "${PCL_IO_INCLUDE_DIR}/pcl/io/openni2_grabber.h") + add_subdirectory(OpenNIGrabber) + endif() endif() set(EXTRA_RTC_DIRS "" CACHE PATH "directories of extra RTCs") diff --git a/rtc/CameraImageLoader/CMakeLists.txt b/rtc/CameraImageLoader/CMakeLists.txt index 98e18107bab..c785f34d46a 100644 --- a/rtc/CameraImageLoader/CMakeLists.txt +++ b/rtc/CameraImageLoader/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(CameraImageLoaderComp ${libs}) set(target CameraImageLoader CameraImageLoaderComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/CameraImageLoader/CameraImageLoader.cpp b/rtc/CameraImageLoader/CameraImageLoader.cpp index 3e50355077a..ee3aed4b058 100644 --- a/rtc/CameraImageLoader/CameraImageLoader.cpp +++ b/rtc/CameraImageLoader/CameraImageLoader.cpp @@ -131,11 +131,16 @@ RTC::ReturnCode_t CameraImageLoader::onExecute(RTC::UniqueId ec_id) m_image.data.image.format = Img::CF_RGB; { // BGR -> RGB - char *src = image->imageData; - for (int i=0; iheight; i++){ + for (int j=0; jwidth; j++){ + src = image->imageData + image->widthStep*i + j*3; + dst[2] = src[0]; + dst[1] = src[1]; + dst[0] = src[2]; + dst += 3; + } } break; } diff --git a/rtc/CameraImageLoader/CameraImageLoader.h b/rtc/CameraImageLoader/CameraImageLoader.h index 7b3d91f34e2..3f9704d8fb7 100644 --- a/rtc/CameraImageLoader/CameraImageLoader.h +++ b/rtc/CameraImageLoader/CameraImageLoader.h @@ -10,13 +10,14 @@ #ifndef CAMERA_IMAGE_LOADER_H #define CAMERA_IMAGE_LOADER_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/CameraImageSaver/CMakeLists.txt b/rtc/CameraImageSaver/CMakeLists.txt new file mode 100644 index 00000000000..ec942ff9d7f --- /dev/null +++ b/rtc/CameraImageSaver/CMakeLists.txt @@ -0,0 +1,15 @@ +set(comp_sources CameraImageSaver.cpp) +set(libs hrpsysBaseStub ${OpenCV_LIBRARIES}) +add_library(CameraImageSaver SHARED ${comp_sources}) +target_link_libraries(CameraImageSaver ${libs}) +set_target_properties(CameraImageSaver PROPERTIES PREFIX "") + +add_executable(CameraImageSaverComp CameraImageSaverComp.cpp ${comp_sources}) +target_link_libraries(CameraImageSaverComp ${libs}) + +set(target CameraImageSaver CameraImageSaverComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/CameraImageSaver/CameraImageSaver.cpp b/rtc/CameraImageSaver/CameraImageSaver.cpp new file mode 100644 index 00000000000..c7a8ca0d3ad --- /dev/null +++ b/rtc/CameraImageSaver/CameraImageSaver.cpp @@ -0,0 +1,219 @@ +// -*- C++ -*- +/*! + * @file CameraImageSaver.cpp + * @brief camera image saver + * $Date$ + * + * $Id$ + */ + +#include +#include +#include "CameraImageSaver.h" + +// Module specification +// +static const char* spec[] = + { + "implementation_id", "CameraImageSaver", + "type_name", "CameraImageSaver", + "description", "camera image saver", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.defalt.basename", "image", + + "" + }; +// + +CameraImageSaver::CameraImageSaver(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_imageIn("image", m_image), + // + m_count(0), + dummy(0) +{ +} + +CameraImageSaver::~CameraImageSaver() +{ +} + + + +RTC::ReturnCode_t CameraImageSaver::onInitialize() +{ + //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("basename", m_basename, "image"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("image", m_imageIn); + + // Set OutPort buffer + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + RTC::Properties& prop = getProperties(); + + return RTC::RTC_OK; +} + + + +/* +RTC::ReturnCode_t CameraImageSaver::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t CameraImageSaver::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t CameraImageSaver::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t CameraImageSaver::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t CameraImageSaver::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t CameraImageSaver::onExecute(RTC::UniqueId ec_id) +{ + //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + if (m_imageIn.isNew()){ + m_imageIn.read(); + + IplImage* cvImage; + switch (m_image.data.image.format){ + case Img::CF_RGB: + cvImage = cvCreateImage(cvSize(m_image.data.image.width, + m_image.data.image.height), + IPL_DEPTH_8U, 3); + break; + case Img::CF_GRAY: + cvImage = cvCreateImage(cvSize(m_image.data.image.width, + m_image.data.image.height), + IPL_DEPTH_8U, 1); + break; + default: + std::cerr << "unsupported color format(" + << m_image.data.image.format << ")" << std::endl; + return RTC::RTC_ERROR; + } + switch(m_image.data.image.format){ + case Img::CF_RGB: + { + // RGB -> BGR + char *dst = cvImage->imageData; + for (unsigned int i=0; iimageData, + m_image.data.image.raw_data.get_buffer(), + m_image.data.image.raw_data.length()); + break; + default: + break; + } + + char fname[256]; + sprintf(fname, "%s%04d.png", m_basename.c_str(), m_count++); + cvSaveImage(fname, cvImage); + + cvReleaseImage(&cvImage); + } + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t CameraImageSaver::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t CameraImageSaver::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t CameraImageSaver::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t CameraImageSaver::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t CameraImageSaver::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + + + +extern "C" +{ + + void CameraImageSaverInit(RTC::Manager* manager) + { + RTC::Properties profile(spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/CameraImageSaver/CameraImageSaver.h b/rtc/CameraImageSaver/CameraImageSaver.h new file mode 100644 index 00000000000..8aa424e1929 --- /dev/null +++ b/rtc/CameraImageSaver/CameraImageSaver.h @@ -0,0 +1,146 @@ +// -*- C++ -*- +/*! + * @file CameraImageSaver.h + * @brief camera image saver + * @date $Date$ + * + * $Id$ + */ + +#ifndef CAMERA_IMAGE_SAVER_H +#define CAMERA_IMAGE_SAVER_H + +#include +#include "hrpsys/idl/Img.hh" +#include +#include +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief sample RT component which has one data input port and one data output port + */ +class CameraImageSaver + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + CameraImageSaver(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~CameraImageSaver(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + Img::TimedCameraImage m_image; + + // DataInPort declaration + // + InPort m_imageIn; + + // + + // DataOutPort declaration + // + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + std::string m_basename; + int m_count; + int dummy; +}; + + +extern "C" +{ + void CameraImageSaverInit(RTC::Manager* manager); +}; + +#endif // CAMERA_IMAGE_SAVER_H diff --git a/rtc/CameraImageSaver/CameraImageSaver.txt b/rtc/CameraImageSaver/CameraImageSaver.txt new file mode 100644 index 00000000000..3ad5f2d5d0d --- /dev/null +++ b/rtc/CameraImageSaver/CameraImageSaver.txt @@ -0,0 +1,52 @@ +/** + +\page CameraImageSaver + +\section introduction Overview + +This component saves images. + + + + +
    implementation_idCameraImageSaver
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + +
    port namedata typeunitdescription
    imageImg::TimedCameraImage
    + +\subsection outports Output Ports + + + +
    port namedata typeunitdescription
    + +\section serviceports Service Ports + +\subsection provider Service Providers + + + +
    port nameinterface nameservice typeIDLdescription
    + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + + +
    nametypeunitdefault valuedescription
    basenamestd::stringimagebasename
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/CameraImageSaver/CameraImageSaverComp.cpp b/rtc/CameraImageSaver/CameraImageSaverComp.cpp new file mode 100644 index 00000000000..745fa030cdd --- /dev/null +++ b/rtc/CameraImageSaver/CameraImageSaverComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file CameraImageSaverComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "CameraImageSaver.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + CameraImageSaverInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("CameraImageSaver"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/CameraImageViewer/CMakeLists.txt b/rtc/CameraImageViewer/CMakeLists.txt index a122c92f300..47c90f68ef4 100644 --- a/rtc/CameraImageViewer/CMakeLists.txt +++ b/rtc/CameraImageViewer/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(CameraImageViewerComp ${libs}) set(target CameraImageViewer CameraImageViewerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/CameraImageViewer/CameraImageViewer.cpp b/rtc/CameraImageViewer/CameraImageViewer.cpp index d8c949bced8..5e48f810389 100644 --- a/rtc/CameraImageViewer/CameraImageViewer.cpp +++ b/rtc/CameraImageViewer/CameraImageViewer.cpp @@ -24,6 +24,7 @@ static const char* cameraimageviewercomponent_spec[] = "language", "C++", "lang_type", "compile", // Configuration variables + "conf.default.depthBits", "11", "" }; @@ -33,6 +34,7 @@ CameraImageViewer::CameraImageViewer(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // m_imageIn("imageIn", m_image), + m_imageOldIn("imageOldIn", m_imageOld), // m_cvImage(NULL), dummy(0) @@ -50,6 +52,7 @@ RTC::ReturnCode_t CameraImageViewer::onInitialize() std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; // // Bind variables and configuration variable + bindParameter("depthBits", m_depthBits, "11"); // @@ -57,6 +60,7 @@ RTC::ReturnCode_t CameraImageViewer::onInitialize() // // Set InPort buffers addInPort("imageIn", m_imageIn); + addInPort("imageOldIn", m_imageOldIn); // Set OutPort buffer @@ -135,6 +139,7 @@ RTC::ReturnCode_t CameraImageViewer::onExecute(RTC::UniqueId ec_id) IPL_DEPTH_8U, 3); break; case Img::CF_GRAY: + case Img::CF_DEPTH: m_cvImage = cvCreateImage(cvSize(m_image.data.image.width, m_image.data.image.height), IPL_DEPTH_8U, 1); @@ -149,11 +154,16 @@ RTC::ReturnCode_t CameraImageViewer::onExecute(RTC::UniqueId ec_id) case Img::CF_RGB: { // RGB -> BGR - char *dst = m_cvImage->imageData; - for (int i=0; iheight; i++){ + for (int j=0; jwidth; j++){ + dst = m_cvImage->imageData + m_cvImage->widthStep*i + j*3; + dst[0] = src[2]; + dst[1] = src[1]; + dst[2] = src[0]; + src += 3; + } } break; } @@ -162,9 +172,59 @@ RTC::ReturnCode_t CameraImageViewer::onExecute(RTC::UniqueId ec_id) m_image.data.image.raw_data.get_buffer(), m_image.data.image.raw_data.length()); break; + case Img::CF_DEPTH: + { + // depth -> gray scale + char *dst = m_cvImage->imageData; + Img::ImageData &id = m_image.data.image; + unsigned short *src = (unsigned short *)id.raw_data.get_buffer(); + int shift = m_depthBits - 8; + for (unsigned int i=0; i>shift; + } + } + break; default: break; } + } + + if (m_imageOldIn.isNew()){ + do { + m_imageOldIn.read(); + }while(m_imageOldIn.isNew()); + if (m_cvImage && (m_imageOld.width != m_cvImage->width + || m_imageOld.height != m_cvImage->height)){ + cvReleaseImage(&m_cvImage); + m_cvImage = NULL; + } + int bytes = m_imageOld.bpp/8; + if (!bytes){ + bytes = m_imageOld.pixels.length()/(m_imageOld.width*m_imageOld.height); + } + if (!m_cvImage){ + m_cvImage = cvCreateImage(cvSize(m_imageOld.width, + m_imageOld.height), + IPL_DEPTH_8U, bytes); + } + switch(bytes){ + case 1: + memcpy(m_cvImage->imageData, + m_imageOld.pixels.get_buffer(), + m_imageOld.pixels.length()); + break; + case 3: + // RGB -> BGR + char *dst = m_cvImage->imageData; + for (unsigned int i=0; i +#include #include #include #include +#include "hrpsys/idl/Img.hh" #include #include #include +#include #include #include -#include "Img.hh" // Service implementation headers // @@ -105,10 +109,12 @@ class CameraImageViewer // Img::TimedCameraImage m_image; + CameraImage m_imageOld; // DataInPort declaration // InPort m_imageIn; + InPort m_imageOldIn; // @@ -134,6 +140,7 @@ class CameraImageViewer private: IplImage* m_cvImage; + int m_depthBits; int dummy; }; diff --git a/rtc/CameraImageViewer/CameraImageViewer.txt b/rtc/CameraImageViewer/CameraImageViewer.txt index 2207f785f7b..61f527eb6b1 100644 --- a/rtc/CameraImageViewer/CameraImageViewer.txt +++ b/rtc/CameraImageViewer/CameraImageViewer.txt @@ -36,7 +36,10 @@ N/A \section configuration Configuration Variables -N/A + + + +
    nametypeunitdefault valuedescription
    depthBitsint[bit]11resolution of depth image
    \section conf Configuration File diff --git a/rtc/CaptureController/CMakeLists.txt b/rtc/CaptureController/CMakeLists.txt index eacc2d5d9db..ad114b10406 100644 --- a/rtc/CaptureController/CMakeLists.txt +++ b/rtc/CaptureController/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(CaptureControllerComp ${libs}) set(target CaptureController CaptureControllerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/CaptureController/CameraCaptureService_impl.h b/rtc/CaptureController/CameraCaptureService_impl.h index 4a08fa6ee67..3f3853efc5b 100644 --- a/rtc/CaptureController/CameraCaptureService_impl.h +++ b/rtc/CaptureController/CameraCaptureService_impl.h @@ -2,7 +2,7 @@ #ifndef __CAMERA_CAPTURE_SERVICE_H__ #define __CAMERA_CAPTURE_SERVICE_H__ -#include "Img.hh" +#include "hrpsys/idl/Img.hh" class CaptureController; diff --git a/rtc/CaptureController/CaptureController.h b/rtc/CaptureController/CaptureController.h index bfde52e6ac3..6ac56634cc7 100644 --- a/rtc/CaptureController/CaptureController.h +++ b/rtc/CaptureController/CaptureController.h @@ -10,6 +10,11 @@ #ifndef CAPTURE_CONTROLLER_H #define CAPTURE_CONTROLLER_H +// Service implementation headers +// +#include "CameraCaptureService_impl.h" + +#include #include #include #include @@ -17,10 +22,6 @@ #include #include -// Service implementation headers -// -#include "CameraCaptureService_impl.h" - // // Service Consumer stub headers diff --git a/rtc/CollisionDetector/CMakeLists.txt b/rtc/CollisionDetector/CMakeLists.txt index 3c5da19e84a..be32a5f5ea5 100644 --- a/rtc/CollisionDetector/CMakeLists.txt +++ b/rtc/CollisionDetector/CMakeLists.txt @@ -1,3 +1,9 @@ +if (APPLE) + include_directories(${PCL_INCLUDE_DIRS}) + link_directories(${PCL_LIBRARY_DIRS}) + add_definitions(${PCL_DEFINITIONS}) +endif() + set(seq_dir ${PROJECT_SOURCE_DIR}/rtc/SequencePlayer) if (USE_HRPSYSUTIL) set(comp_sources ${seq_dir}/interpolator.cpp CollisionDetector.cpp CollisionDetectorService_impl.cpp GLscene.cpp VclipLinkPair.cpp ../SoftErrorLimiter/beep.cpp) @@ -45,6 +51,6 @@ else() endif() install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/CollisionDetector/CollisionDetector.cpp b/rtc/CollisionDetector/CollisionDetector.cpp index 1e5d592f07f..cd36e1ad4a5 100644 --- a/rtc/CollisionDetector/CollisionDetector.cpp +++ b/rtc/CollisionDetector/CollisionDetector.cpp @@ -15,14 +15,13 @@ #include #include #ifdef USE_HRPSYSUTIL -#include "util/GLbody.h" -#include "util/GLutil.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLutil.h" #endif // USE_HRPSYSUTIL -#include "util/BVutil.h" -#include "RobotHardwareService.hh" +#include "hrpsys/util/BVutil.h" +#include "hrpsys/idl/RobotHardwareService.hh" #include "CollisionDetector.h" -#include "../SoftErrorLimiter/beep.h" #define deg2rad(x) ((x)*M_PI/180) #define rad2deg(x) ((x)*180/M_PI) @@ -47,6 +46,17 @@ static const char* component_spec[] = }; // +static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm) +{ + int pre = os.precision(); + os.setf(std::ios::fixed); + os << std::setprecision(6) + << (tm.sec + tm.nsec/1e9) + << std::setprecision(pre); + os.unsetf(std::ios::fixed); + return os; +} + CollisionDetector::CollisionDetector(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // @@ -54,23 +64,23 @@ CollisionDetector::CollisionDetector(RTC::Manager* manager) m_qCurrentIn("qCurrent", m_qCurrent), m_servoStateIn("servoStateIn", m_servoState), m_qOut("q", m_q), + m_beepCommandOut("beepCommand", m_beepCommand), m_CollisionDetectorServicePort("CollisionDetectorService"), // - m_loop_for_check(0), - m_collision_loop(1), - m_use_limb_collision(false), #ifdef USE_HRPSYSUTIL + m_scene(&m_log), + m_window(&m_scene, &m_log), m_glbody(NULL), #endif // USE_HRPSYSUTIL + m_use_limb_collision(false), m_use_viewer(false), m_robot(hrp::BodyPtr()), -#ifdef USE_HRPSYSUTIL - m_scene(&m_log), - m_window(&m_scene, &m_log), -#endif // USE_HRPSYSUTIL + m_loop_for_check(0), + m_collision_loop(1), m_debugLevel(0), m_enable(true), collision_beep_count(0), + is_beep_port_connected(false), dummy(0) { m_service0.collision(this); @@ -106,6 +116,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() // Set OutPort buffer addOutPort("q", m_qOut); + addOutPort("beepCommand", m_beepCommandOut); // Set service provider to Ports m_CollisionDetectorServicePort.registerProvider("service0", "CollisionDetectorService", m_service0); @@ -180,7 +191,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() if ( m_robot->link(name1)==NULL ) { std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name1 << std::endl; std::cerr << "[" << m_profile.instance_name << "] please choose one of following :"; - for (int i=0; i < m_robot->numLinks(); i++) { + for (unsigned int i=0; i < m_robot->numLinks(); i++) { std::cerr << " " << m_robot->link(i)->name; } std::cerr << std::endl; @@ -189,7 +200,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() if ( m_robot->link(name2)==NULL ) { std::cerr << "[" << m_profile.instance_name << "] Could not find robot link " << name2 << std::endl; std::cerr << "[" << m_profile.instance_name << "] please choose one of following :"; - for (int i=0; i < m_robot->numLinks(); i++) { + for (unsigned int i=0; i < m_robot->numLinks(); i++) { std::cerr << " " << m_robot->link(i)->name; } std::cerr << std::endl; @@ -258,12 +269,12 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() m_interpolator->setName(std::string(m_profile.instance_name)+" interpolator"); m_link_collision = new bool[m_robot->numLinks()]; - for(int i=0; inumJoints(); i++){ + for(unsigned int i=0; inumJoints(); i++){ m_q.data[i] = 0; } m_servoState.data.length(m_robot->numJoints()); - for(int i = 0; i < m_robot->numJoints(); i++) { + for(unsigned int i = 0; i < m_robot->numJoints(); i++) { m_servoState.data[i].length(1); int status = 0; status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT; @@ -275,6 +286,7 @@ RTC::ReturnCode_t CollisionDetector::onInitialize() } collision_beep_freq = static_cast(1.0/(3.0*m_dt)); // 3 times / 1[s] + m_beepCommand.data.length(bc.get_num_beep_info()); return RTC::RTC_OK; } @@ -321,6 +333,18 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) { static int loop = 0; loop++; + + // Connection check of m_beepCommand to BeeperRTC + // If m_beepCommand is not connected to BeeperRTC and sometimes, check connection. + // If once connection is found, never check connection. + if (!is_beep_port_connected && (loop % 500 == 0) ) { + if ( m_beepCommandOut.connectors().size() > 0 ) { + is_beep_port_connected = true; + quit_beep(); + std::cerr << "[" << m_profile.instance_name<< "] beepCommand data port connection found! Use BeeperRTC." << std::endl; + } + } + if (m_servoStateIn.isNew()) { m_servoStateIn.read(); } @@ -330,7 +354,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) } if ( m_qRefIn.isNew()) { m_qRefIn.read(); - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { m_q.data[i] = m_qRef.data[i]; } m_q.tm = m_qRef.tm; @@ -340,40 +364,49 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) if (m_enable && m_qRefIn.isNew()) { m_qRefIn.read(); + // check servo for collision beep sound + bool has_servoOn = false; + for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; + has_servoOn = has_servoOn || (servo_state == 1); + } + TimedPosture tp; assert(m_qRef.data.length() == m_robot->numJoints()); #ifdef USE_HRPSYSUTIL if ( m_use_viewer ) { - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ ((GLlink *)m_glbody->link(i))->highlight(false); } } - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ m_link_collision[m_glbody->link(i)->index] = false; } #else - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ m_link_collision[m_robot->link(i)->index] = false; } #endif // USE_HRPSYSUTIL //set robot model's angle for collision check(two types) - // 1. current safe angle .. check based on qRef - // 2. recovery or collision angle .. check based on q'(m_recover_jointdata) - if (m_safe_posture && m_recover_time == 0) { // 1. current safe angle + // 1. current safe angle or collision angle .. check based on qRef + // 2. recovery .. check based on q'(m_recover_jointdata) + if (m_recover_time == 0 || m_recover_time == default_recover_time ) { // 1. current safe angle or collision angle if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qRef.data[i]; } } - }else{ // recovery or collision angle - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + }else{ // 2. recovery + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + if ( m_loop_for_check == 0 ) { // update robot posutre for each m_loop_for_check timing if ( m_curr_collision_mask[i] == 1) {// joint with 1 (do not move when collide :default), need to be updated using recover(safe) data m_robot->joint(i)->q = m_recover_jointdata[i]; }else{ // joint with 0 (move even if collide), need to be updated using reference(dangerous) data m_robot->joint(i)->q = m_qRef.data[i]; } + } } } // } @@ -381,7 +414,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) m_robot->calcForwardKinematics(); coil::TimeValue tm1 = coil::gettimeofday(); std::map::iterator it = m_pair.begin(); - for (unsigned int i = 0; it != m_pair.end(); it++, i++){ + for (int i = 0; it != m_pair.end(); it++, i++){ int sub_size = (m_pair.size() + m_collision_loop -1) / m_collision_loop; // 10 / 3 = 3 / floor // 0 : 0 .. sub_size-1 // 0 .. 2 // 1 : sub_size ... sub_size*2-1 // 3 .. 5 @@ -390,7 +423,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) if ( sub_size*m_loop_for_check <= i && i < sub_size*(m_loop_for_check+1) ) { CollisionLinkPair* c = it->second; c->distance = c->pair->computeDistance(c->point0.data(), c->point1.data()); - //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << " "; + //std::cerr << i << ":" << (c->distance<=c->pair->getTolerance() ) << "/" << c->distance << " "; } } if ( m_loop_for_check == m_collision_loop-1 ) { @@ -414,8 +447,8 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) bool stop_all = true; // if all joint is within false(0:move even if collide) in initial mask ( for example leg to leg ) we stop them // if some joint is not within true(1:do not move within collide) on initial mask, stop only true joint (for exmple leg to arm) - for ( int i = 0; i < jointPath->numJoints(); i++ ){ if ( m_init_collision_mask[jointPath->joint(i)->jointId] == 1) stop_all = false; } - for ( int i = 0; i < jointPath->numJoints(); i++ ){ + for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){ if ( m_init_collision_mask[jointPath->joint(i)->jointId] == 1) stop_all = false; } + for ( unsigned int i = 0; i < jointPath->numJoints(); i++ ){ int id = jointPath->joint(i)->jointId; // collision_mask used to select output 0: passthough reference data, 1 output safe data if ( stop_all ) { @@ -436,12 +469,27 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) } } if ( m_safe_posture ) { + if (has_servoOn) { + if (! m_have_safe_posture ) { + // first transition collision -> safe + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] set safe posture" << std::endl; + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { + m_lastsafe_jointdata[i] = m_robot->joint(i)->q; + } + m_interpolator->set(m_lastsafe_jointdata); // Set current angles as initial angle for recover + } m_have_safe_posture = true; - for ( int i = 0; i < m_q.data.length(); i++ ) { - m_lastsafe_jointdata[i] = m_robot->joint(i)->q; + } + if (m_recover_time != default_recover_time) { + // sefe or recover + // in collision, robot->q may differ from m_q. + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { + m_lastsafe_jointdata[i] = m_robot->joint(i)->q; + } } }else{ - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { if ( m_curr_collision_mask[i] == 0 ) { // if collisoin_mask is 0 (move even if collide), we update lastsafe_joint_data from input data m_lastsafe_jointdata[i] = m_robot->joint(i)->q; } @@ -465,26 +513,31 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) coil::TimeValue tm2 = coil::gettimeofday(); if (m_safe_posture && m_recover_time == 0){ // safe mode //std::cerr << "safe-------------- " << std::endl; - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { m_q.data[i] = m_qRef.data[i]; } // collision_mask used to select output 0: passthough reference data, 1 output safe data std::fill(m_curr_collision_mask.begin(), m_curr_collision_mask.end(), 0); // false(0) clear output data } else { + static int collision_loop_recover = 0; if(m_safe_posture){ //recover //std::cerr << "recover-------------- " << std::endl; - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { m_q.data[i] = m_recover_jointdata[i]; } m_recover_time = m_recover_time - i_dt; - }else{ //collision + } else if (m_collision_loop > 1 && (m_recover_time != default_recover_time)) { // collision with collision_loop + //std::cerr << "collision_loop-------------- " << std::endl; + collision_loop_recover = m_collision_loop; + m_recover_time = default_recover_time; // m_recover_time should be set based on difference between qRef and q + } else { //collision //std::cerr << "collision-------------- " << std::endl; //do nothing (stay previous m_q) m_recover_time = default_recover_time; // m_recover_time should be set based on difference between qRef and q m_interpolator->set(m_lastsafe_jointdata); //Set last safe joint data as initial angle //m_interpolator->set(m_q.data.get_buffer()); //Set initial angle } - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { if (m_curr_collision_mask[i] == 0) { // 0: passthough reference data, 1 output safe data, stop joints only joint with 1 m_q.data[i] = m_qRef.data[i]; } @@ -496,9 +549,18 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) m_recover_jointdata[i] = m_q.data[i] + (m_qRef.data[i] - m_q.data[i]) / m_recover_time; } #else + if (collision_loop_recover != 0) { + collision_loop_recover--; + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { + m_q.data[i] = + m_lastsafe_jointdata[i] + collision_loop_recover * ((m_q.data[i] - m_lastsafe_jointdata[i])/m_collision_loop); + } + } else { + collision_loop_recover = 0; //minjerk interpolation m_interpolator->setGoal(m_qRef.data.get_buffer(), m_recover_time); m_interpolator->get(m_recover_jointdata); + } #endif } if ( DEBUGP ) { @@ -512,7 +574,7 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) if ( DEBUGP || (loop % ((int)(5/m_dt))) == 1) { std::cerr << "[" << m_profile.instance_name << "] CAUTION!! The robot is moving while collision detection!!!, since we do not get safe_posture yet" << std::endl; } - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { m_lastsafe_jointdata[i] = m_recover_jointdata[i] = m_q.data[i] = m_qRef.data[i]; } } @@ -520,23 +582,22 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) m_q.tm = m_qRef.tm; m_qOut.write(); - // beep sound for collision alert - // check servo for collision beep sound - bool has_servoOn = false; - for (int i = 0; i < m_robot->numJoints(); i++ ){ - int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; - has_servoOn = has_servoOn || (servo_state == 1); - } // if servo off, we do not know last safe posture if (! has_servoOn ) { m_have_safe_posture = false; } - // beep + // beep sound for collision alert if ( !m_safe_posture && has_servoOn ) { // If collided and some joint is servoOn - if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) start_beep(2352, collision_beep_freq*0.7); - else stop_beep(); + if (is_beep_port_connected) { + if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) bc.startBeep(2352, collision_beep_freq*0.7); + else bc.stopBeep(); + } else { + if ( collision_beep_count % collision_beep_freq == 0 && collision_beep_count % (collision_beep_freq * 3) != 0 ) start_beep(2352, collision_beep_freq*0.7); + else stop_beep(); + } collision_beep_count++; } else { + if (is_beep_port_connected) bc.stopBeep(); collision_beep_count = 0; } @@ -549,17 +610,17 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) // set collisoin state m_state.time = tm2; - for (int i = 0; i < m_robot->numJoints(); i++ ){ + for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_state.angle[i] = m_robot->joint(i)->q; } if ( m_loop_for_check == 0 ) { - for (int i = 0; i < m_robot->numLinks(); i++ ){ + for (unsigned int i = 0; i < m_robot->numLinks(); i++ ){ m_state.collide[i] = m_link_collision[i]; } m_state.lines.length(tp.lines.size()); - for(int i = 0; i < tp.lines.size(); i++ ){ + for(unsigned int i = 0; i < tp.lines.size(); i++ ){ const std::pair& line = tp.lines[i]; double *v; m_state.lines[i].length(2); @@ -580,6 +641,10 @@ RTC::ReturnCode_t CollisionDetector::onExecute(RTC::UniqueId ec_id) m_state.recover_time = m_recover_time; m_state.loop_for_check = m_loop_for_check; } + if (is_beep_port_connected) { + bc.setDataPort(m_beepCommand); + if (bc.isWritable()) m_beepCommandOut.write(); + } #ifdef USE_HRPSYSUTIL if ( m_use_viewer ) m_window.oneStep(); #endif // USE_HRPSYSUTIL @@ -634,6 +699,14 @@ bool CollisionDetector::setTolerance(const char *i_link_pair_name, double i_tole return true; } +bool CollisionDetector::setCollisionLoop(int input_loop) { + if (input_loop > 0) { + m_collision_loop = input_loop; + return true; + } + return false; +} + bool CollisionDetector::getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState &state) { state = m_state; @@ -644,7 +717,7 @@ void CollisionDetector::setupVClipModel(hrp::BodyPtr i_body) { m_VclipLinks.resize(i_body->numLinks()); //std::cerr << i_body->numLinks() << std::endl; - for (int i=0; inumLinks(); i++) { + for (unsigned int i=0; inumLinks(); i++) { assert(i_body->link(i)->index == i); setupVClipModel(i_body->link(i)); } @@ -652,7 +725,7 @@ void CollisionDetector::setupVClipModel(hrp::BodyPtr i_body) bool CollisionDetector::checkIsSafeTransition(void) { - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { // If servoOn, check too large joint angle gap. Otherwise (servoOff), neglect too large joint angle gap. int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; // enum SwitchStatus {SWITCH_ON, SWITCH_OFF}; if (servo_state == 1 && abs(m_q.data[i] - m_qRef.data[i]) > 0.017) return false; @@ -673,7 +746,7 @@ bool CollisionDetector::enable(void) } // check collision - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qRef.data[i]; } m_robot->calcForwardKinematics(); @@ -722,6 +795,8 @@ void CollisionDetector::setupVClipModel(hrp::Link *i_link) } i_vclip_model->buildHull(); i_vclip_model->check(); + fprintf(stderr, "[Vclip] build finished, vcliip mesh of %s, %d -> %d\n", + i_link->name.c_str(), n, (int)(i_vclip_model->verts().size())); m_VclipLinks[i_link->index] = i_vclip_model; } diff --git a/rtc/CollisionDetector/CollisionDetector.h b/rtc/CollisionDetector/CollisionDetector.h index 4634660066f..ec352f09a3e 100644 --- a/rtc/CollisionDetector/CollisionDetector.h +++ b/rtc/CollisionDetector/CollisionDetector.h @@ -10,6 +10,8 @@ #ifndef COLLISION_DETECTOR_H #define COLLISION_DETECTOR_H +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include @@ -21,15 +23,15 @@ #include #ifdef USE_HRPSYSUTIL #include "GLscene.h" -#include "util/SDLUtil.h" -#include "util/LogManager.h" +#include "hrpsys/util/SDLUtil.h" +#include "hrpsys/util/LogManager.h" #endif // USE_HRPSYSUTIL #include "TimedPosture.h" #include "interpolator.h" -#include "HRPDataTypes.hh" #include "VclipLinkPair.h" #include "CollisionDetectorService_impl.h" +#include "../SoftErrorLimiter/beep.h" // Service implementation headers // @@ -109,6 +111,7 @@ class CollisionDetector // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); bool setTolerance(const char *i_link_pair_name, double i_tolerance); + bool setCollisionLoop(int input_loop); bool getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState &state); bool checkIsSafeTransition(void); @@ -136,6 +139,8 @@ class CollisionDetector // TimedDoubleSeq m_q; OutPort m_qOut; + TimedLongSeq m_beepCommand; + OutPort m_beepCommandOut; // @@ -185,7 +190,6 @@ class CollisionDetector bool m_safe_posture; int m_recover_time; double m_dt; - int dummy; // double *m_recover_jointdata, *m_lastsafe_jointdata; bool *m_link_collision; @@ -197,6 +201,11 @@ class CollisionDetector int collision_beep_freq, collision_beep_count; bool m_have_safe_posture; OpenHRP::CollisionDetectorService::CollisionState m_state; + BeepClient bc; + // Since this RTC is stable RTC, we support both direct beeping from this RTC and beepring through BeeperRTC. + // If m_beepCommand is connected to BeeperRTC, is_beep_port_connected is true. + bool is_beep_port_connected; + int dummy; }; #ifndef USE_HRPSYSUTIL diff --git a/rtc/CollisionDetector/CollisionDetector.txt b/rtc/CollisionDetector/CollisionDetector.txt index b8640284e4c..51a424b06d5 100644 --- a/rtc/CollisionDetector/CollisionDetector.txt +++ b/rtc/CollisionDetector/CollisionDetector.txt @@ -24,6 +24,10 @@ Transition from overwrite mode to non-overwrite mode is implemented as min-jerk interplation by default. Transition time is 1.0[s] by default. +In collision, this component makes beep sound. +If the cause of errors are removed, error flags and beep sound are removed. +Since this RTC is stable RTC, we support both direct beeping from this RTC and beepring through BeeperRTC. + \section dataports Data Ports \subsection inports Input Ports diff --git a/rtc/CollisionDetector/CollisionDetectorService_impl.cpp b/rtc/CollisionDetector/CollisionDetectorService_impl.cpp index c41b76cf31f..acfccd71b6c 100644 --- a/rtc/CollisionDetector/CollisionDetectorService_impl.cpp +++ b/rtc/CollisionDetector/CollisionDetectorService_impl.cpp @@ -24,6 +24,11 @@ CORBA::Boolean CollisionDetectorService_impl::setTolerance(const char *i_link_pa return m_collision->setTolerance(i_link_pair_name, d_tolerance); } +CORBA::Boolean CollisionDetectorService_impl::setCollisionLoop(CORBA::Short loop) +{ + return m_collision->setCollisionLoop(loop); +} + CORBA::Boolean CollisionDetectorService_impl::getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState_out state) { state = new OpenHRP::CollisionDetectorService::CollisionState; diff --git a/rtc/CollisionDetector/CollisionDetectorService_impl.h b/rtc/CollisionDetector/CollisionDetectorService_impl.h index 1d4fac4e905..91ef720444e 100644 --- a/rtc/CollisionDetector/CollisionDetectorService_impl.h +++ b/rtc/CollisionDetector/CollisionDetectorService_impl.h @@ -2,7 +2,7 @@ #ifndef COLLISIONDETECTORSERVICE_IMPL_H #define COLLISIONDETECTORSERVICE_IMPL_H -#include "CollisionDetectorService.hh" +#include "hrpsys/idl/CollisionDetectorService.hh" using namespace OpenHRP; @@ -19,6 +19,7 @@ class CollisionDetectorService_impl CORBA::Boolean enableCollisionDetection(); CORBA::Boolean disableCollisionDetection(); CORBA::Boolean setTolerance(const char *i_link_pair_name, CORBA::Double d_tolerance); + CORBA::Boolean setCollisionLoop(CORBA::Short loop); CORBA::Boolean getCollisionStatus(OpenHRP::CollisionDetectorService::CollisionState_out state); void collision(CollisionDetector *i_collision); // diff --git a/rtc/CollisionDetector/CollisionDetectorViewer.cpp b/rtc/CollisionDetector/CollisionDetectorViewer.cpp index 7cb0ee59c33..8940d4abd5d 100644 --- a/rtc/CollisionDetector/CollisionDetectorViewer.cpp +++ b/rtc/CollisionDetector/CollisionDetectorViewer.cpp @@ -7,16 +7,16 @@ #else #include #endif -#include "util/ProjectUtil.h" -#include "util/GLbody.h" -#include "util/GLlink.h" -#include "util/GLutil.h" -#include "util/SDLUtil.h" -#include "util/LogManager.h" -#include "util/BVutil.h" +#include "hrpsys/util/ProjectUtil.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" +#include "hrpsys/util/SDLUtil.h" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/util/BVutil.h" #include "TimedPosture.h" #include "GLscene.h" -#include "CollisionDetectorService.hh" +#include "hrpsys/idl/CollisionDetectorService.hh" using namespace hrp; using namespace OpenHRP; @@ -59,7 +59,7 @@ int main(int argc, char* argv[]) return 1; } - char *coname = "co"; + const char *coname = "co"; int wsize = 0; float bgColor[] = {0,0,0}; for (int i = 2; i #endif #include -#include "util/GLcamera.h" -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/LogManager.h" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/LogManager.h" #include "TimedPosture.h" #include "GLscene.h" -#include "CollisionDetectorService.hh" +#include "hrpsys/idl/CollisionDetectorService.hh" using namespace OpenHRP; @@ -37,7 +37,7 @@ void GLscene::updateScene() GLbody *glbody = dynamic_cast(body(0).get()); OpenHRP::CollisionDetectorService::CollisionState &co = lm->state(); if (co.angle.length() == glbody->numJoints()){ - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ GLlink *j = (GLlink *)glbody->joint(i); if (j){ j->setQ(co.angle[i]); @@ -50,7 +50,7 @@ void GLscene::updateScene() GLbody *glbody = dynamic_cast(body(0).get()); TimedPosture &ts = lm->state(); if (ts.posture.size() == glbody->numJoints()){ - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ GLlink *j = (GLlink *)glbody->joint(i); if (j){ j->setQ(ts.posture[i]); @@ -135,7 +135,7 @@ void GLscene::showStatus() int height = m_height-HEIGHT_STEP; int x = width; - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ hrp::Link *l = glbody->link(i); if (l){ sprintf(buf, "%13s %4d tris", diff --git a/rtc/CollisionDetector/GLscene.h b/rtc/CollisionDetector/GLscene.h index e51cca29828..643e9562831 100644 --- a/rtc/CollisionDetector/GLscene.h +++ b/rtc/CollisionDetector/GLscene.h @@ -2,7 +2,7 @@ #define __GLSCENE_H__ #include -#include "util/GLsceneBase.h" +#include "hrpsys/util/GLsceneBase.h" class LogManagerBase; diff --git a/rtc/CollisionDetector/SetupCollisionPair.cpp b/rtc/CollisionDetector/SetupCollisionPair.cpp index 44d2e0fa0b1..0e8b755f214 100644 --- a/rtc/CollisionDetector/SetupCollisionPair.cpp +++ b/rtc/CollisionDetector/SetupCollisionPair.cpp @@ -16,7 +16,11 @@ #include #include "CollisionDetector.h" extern "C" { +#if (defined __APPLE__) +#include +#else #include +#endif } #define deg2rad(x) ((x)*M_PI/180) @@ -30,7 +34,7 @@ RTC::ReturnCode_t setupCollisionModel(hrp::BodyPtr m_robot, const char *url, Ope // do qhull OpenHRP::ShapeInfoSequence_var sis = binfo->shapes(); OpenHRP::LinkInfoSequence_var lis = binfo->links(); - for(int i = 0; i < m_robot->numLinks(); i++ ) { + for(unsigned int i = 0; i < m_robot->numLinks(); i++ ) { const OpenHRP::LinkInfo& i_li = lis[i]; const OpenHRP::TransformedShapeIndexSequence& tsis = i_li.shapeIndices; // setup @@ -134,7 +138,7 @@ RTC::ReturnCode_t setupCollisionModel(hrp::BodyPtr m_robot, const char *url, Ope bool checkCollisionForAllJointRange(int i, hrp::JointPathPtr jointPath, std::vector &collisionPairs) { - if ( i >= jointPath->numJoints() ) return false; + if ( i >= (int)jointPath->numJoints() ) return false; if ( collisionPairs.size() == 0 ) return true; hrp::Link *l = jointPath->joint(i); @@ -197,9 +201,9 @@ void setupCollisionLinkPair() // need AABBCollision? // // set all collisoin pair - for (int i=0; inumLinks(); i++) { + for (unsigned int i=0; inumLinks(); i++) { hrp::Link *l1 = m_robot->link(i); - for (int j=i+1; jnumLinks(); j++) { + for (unsigned int j=i+1; jnumLinks(); j++) { hrp::Link *l2 = m_robot->link(j); if ( l1->coldetModel && l2->coldetModel && (!(checkBlackListJoint(l1) || checkBlackListJoint(l2) || checkBlackListJointPair(l1, l2))) @@ -289,7 +293,7 @@ void setupCollisionLinkPair() hrp::JointPathPtr jointPath2 = m_robot->getJointPath(((*ii).first)->link(0),((*ii).first)->link(1)); // check if JointPath1 is included in jointPath2 bool find_key = true; - for (int j = 0; j < jointPath1->numJoints() ; j++ ) { + for (unsigned int j = 0; j < jointPath1->numJoints() ; j++ ) { if ( jointPath1->joint(j)->name != jointPath2->joint(j)->name ) { find_key = false; break; @@ -319,7 +323,7 @@ void setupCollisionLinkPair() i += (*ii).second.size(); for(std::vector::iterator it = sub_pairs.begin(); it != sub_pairs.end(); it++ ) { hrp::JointPathPtr jointPath = m_robot->getJointPath((*it)->link(0),(*it)->link(1)); - for ( int j = 0; j < jointPath->numJoints(); j++ ) { + for ( unsigned int j = 0; j < jointPath->numJoints(); j++ ) { std::cerr << jointPath->joint(j)->name << " "; } std::cerr << std::endl; diff --git a/rtc/CollisionDetector/vclip_1.0/src/PolyTree.C b/rtc/CollisionDetector/vclip_1.0/src/PolyTree.C index 0e477a3e6ec..8e91ed69d44 100644 --- a/rtc/CollisionDetector/vclip_1.0/src/PolyTree.C +++ b/rtc/CollisionDetector/vclip_1.0/src/PolyTree.C @@ -58,7 +58,11 @@ using namespace Vclip; #if QHULL extern "C" { +#if (defined __APPLE__) +#include +#else #include "qhull/qhull_a.h" +#endif } //char qh_version[] = "vclip 1.0"; #endif @@ -94,7 +98,7 @@ ostream& Plane::print(ostream &os) const oldFlags = os.setf(ios::showpos); os << normal_.x << " x " << normal_.y << " y " << normal_.z << " z " << offset_ << " >= 0"; - os.flags((std::_Ios_Fmtflags)oldFlags); + os.flags((std::ios_base::fmtflags)oldFlags); return os; } @@ -617,7 +621,7 @@ int Polyhedron::buildHull() npts = verts_.size(); - if (npts > hullVerts.capacity()) { + if (npts > (int)hullVerts.capacity()) { array.reserve(3 * npts); hullVerts.reserve(npts); vertUsed.reserve(npts); @@ -970,7 +974,7 @@ const PolyTree *PolyTreeLibrary::lookup(int i) const { list< Handle >::const_iterator libi; - if (i < 0 || i >= lib.size()) return NULL; + if (i < 0 || i >= (int)lib.size()) return NULL; for (libi = lib.begin(); i-- > 0; ++libi); return &**libi; } diff --git a/rtc/CollisionDetector/vclip_1.0/src/mv.C b/rtc/CollisionDetector/vclip_1.0/src/mv.C index 4d274ec38f3..7c3854f82bd 100644 --- a/rtc/CollisionDetector/vclip_1.0/src/mv.C +++ b/rtc/CollisionDetector/vclip_1.0/src/mv.C @@ -81,7 +81,7 @@ ostream& Vect3::print(ostream &os) const { int oldFlags = os.setf(ios::showpos); os << '(' << x << ' ' << y << ' ' << z << ')'; - os.flags((std::_Ios_Fmtflags)oldFlags); + os.flags((std::ios_base::fmtflags)oldFlags); return os; } @@ -143,7 +143,7 @@ ostream& Mat3::print(ostream &os) const os << '[' << xx << ' ' << xy << ' ' << xz << ']' << endl; os << '[' << yx << ' ' << yy << ' ' << yz << ']' << endl; os << '[' << zx << ' ' << zy << ' ' << zz << ']' << endl; - os.flags((std::_Ios_Fmtflags)oldFlags); + os.flags((std::ios_base::fmtflags)oldFlags); return os; } @@ -473,7 +473,7 @@ ostream& Quat::print(ostream &os) const { int oldFlags = os.setf(ios::showpos); os << '(' << s_ << ' ' << x_ << ' ' << y_ << ' ' << z_ << ')'; - os.flags((std::_Ios_Fmtflags)oldFlags); + os.flags((std::ios_base::fmtflags)oldFlags); return os; } diff --git a/rtc/CollisionDetector/vclip_1.0/src/vclip.C b/rtc/CollisionDetector/vclip_1.0/src/vclip.C index ac8d7d65226..e58d72acbee 100644 --- a/rtc/CollisionDetector/vclip_1.0/src/vclip.C +++ b/rtc/CollisionDetector/vclip_1.0/src/vclip.C @@ -473,7 +473,7 @@ int Polyhedron::edgeFaceTest(const Feature *&e, const Feature *&f, static vector code(MAX_VERTS_PER_FACE); //(template can't use local type) static vector lam(MAX_VERTS_PER_FACE); - if (F(f)->sides > code.capacity()) { + if (F(f)->sides > (int)code.capacity()) { code.reserve(F(f)->sides); lam.reserve(F(f)->sides); } diff --git a/rtc/ColorExtractor/CMakeLists.txt b/rtc/ColorExtractor/CMakeLists.txt new file mode 100644 index 00000000000..66feec05d1e --- /dev/null +++ b/rtc/ColorExtractor/CMakeLists.txt @@ -0,0 +1,15 @@ +set(comp_sources ColorExtractor.cpp) +set(libs ${OpenCV_LIBRARIES} hrpsysBaseStub) +add_library(ColorExtractor SHARED ${comp_sources}) +target_link_libraries(ColorExtractor ${libs}) +set_target_properties(ColorExtractor PROPERTIES PREFIX "") + +add_executable(ColorExtractorComp ColorExtractorComp.cpp ${comp_sources}) +target_link_libraries(ColorExtractorComp ${libs}) + +set(target ColorExtractor ColorExtractorComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/ColorExtractor/ColorExtractor.cpp b/rtc/ColorExtractor/ColorExtractor.cpp new file mode 100644 index 00000000000..3b66d19adba --- /dev/null +++ b/rtc/ColorExtractor/ColorExtractor.cpp @@ -0,0 +1,252 @@ +// -*- C++ -*- +/*! + * @file ColorExtractor.cpp + * @brief rotate image component + * $Date$ + * + * $Id$ + */ + +#include +#include "ColorExtractor.h" +#include "hrpsys/util/VectorConvert.h" + +// Module specification +// +static const char* spec[] = + { + "implementation_id", "ColorExtractor", + "type_name", "ColorExtractor", + "description", "rotate image component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.minPixels", "0", + "conf.default.rgbRegion", "0,0,0,0,0,0", + + "" + }; +// + +ColorExtractor::ColorExtractor(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_originalIn("original", m_original), + m_resultOut("result", m_result), + m_posOut("pos", m_pos), + // + m_img(NULL), + dummy(0) +{ +} + +ColorExtractor::~ColorExtractor() +{ + if (m_img) cvReleaseImage(&m_img); + +} + + + +RTC::ReturnCode_t ColorExtractor::onInitialize() +{ + std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("minPixels", m_minPixels, "0"); + bindParameter("rgbRegion", m_rgbRegion, "0,0,0,0,0,0"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("original", m_originalIn); + + // Set OutPort buffer + addOutPort("result", m_resultOut); + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + //RTC::Properties& prop = getProperties(); + + return RTC::RTC_OK; +} + + + +/* +RTC::ReturnCode_t ColorExtractor::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ColorExtractor::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ColorExtractor::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ColorExtractor::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ColorExtractor::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ColorExtractor::onExecute(RTC::UniqueId ec_id) +{ + //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + if (m_originalIn.isNew()){ + m_originalIn.read(); + + Img::ImageData& idat = m_original.data.image; + + if (m_img && (m_img->width != idat.width + || m_img->height != idat.height)){ + cvReleaseImage(&m_img); + m_img = NULL; + } + if (!m_img){ + m_img = cvCreateImage(cvSize(idat.width, idat.height), + IPL_DEPTH_8U, 3); + m_result.data.image.width = idat.width; + m_result.data.image.height = idat.height; + m_result.data.image.format = idat.format; + m_result.data.image.raw_data.length(idat.width*idat.height*3); + } + + // RGB -> BGR + unsigned char *rtm=idat.raw_data.get_buffer(); + char *cv; + for (int i=0; iimageData + m_img->widthStep*i + j*3; + cv[0] = rtm[2]; + cv[1] = rtm[1]; + cv[2] = rtm[0]; + rtm += 3; + } + } + + // processing start + int npixel=0, cx=0, cy=0; + unsigned char b,g,r; + for (int i=0; iimageData + m_img->widthStep*i + j*3; + b = cv[0]; g = cv[1]; r = cv[2]; + if (r > m_rgbRegion[0] && g > m_rgbRegion[1] && b > m_rgbRegion[2] + && r < m_rgbRegion[3] && g < m_rgbRegion[4] && b < m_rgbRegion[5]){ + cx += j; + cy += i; + npixel++; + }else{ + } + } + } + if (npixel > 10){ + cx /= npixel; + cy /= npixel; + //printf("cx=%d, cy=%d, npixel=%d\n", cx, cy, npixel); + cvCircle(m_img, cvPoint(cx, cy), sqrt(npixel), CV_RGB(0,0,255), 6, 8, 0); + m_pos.tm = m_original.tm; + m_pos.data.x = cx; + m_pos.data.y = cy; + m_posOut.write(); + } + // processing end + + // BGR -> RGB + rtm = m_result.data.image.raw_data.get_buffer(); + for (int i=0; iimageData + m_img->widthStep*i + j*3; + rtm[2] = cv[0]; + rtm[1] = cv[1]; + rtm[0] = cv[2]; + rtm += 3; + } + } + + m_resultOut.write(); + } + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ColorExtractor::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ColorExtractor::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ColorExtractor::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ColorExtractor::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ColorExtractor::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + + + +extern "C" +{ + + void ColorExtractorInit(RTC::Manager* manager) + { + RTC::Properties profile(spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/ColorExtractor/ColorExtractor.h b/rtc/ColorExtractor/ColorExtractor.h new file mode 100644 index 00000000000..a0dfb4e1c91 --- /dev/null +++ b/rtc/ColorExtractor/ColorExtractor.h @@ -0,0 +1,154 @@ +// -*- C++ -*- +/*! + * @file ColorExtractor.h + * @brief rotate image component + * @date $Date$ + * + * $Id$ + */ + +#ifndef COLOR_EXTRACTOR_H +#define COLOR_EXTRACTOR_H + +#include +#include +#include "hrpsys/idl/Img.hh" +#include +#include +#include +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief RT component which resize an input image + */ +class ColorExtractor + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + ColorExtractor(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~ColorExtractor(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + Img::TimedCameraImage m_original; + + // DataInPort declaration + // + InPort m_originalIn; + + // + + Img::TimedCameraImage m_result; + RTC::TimedPoint2D m_pos; + + // DataOutPort declaration + // + OutPort m_resultOut; + OutPort m_posOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + IplImage *m_img; + int m_minPixels; + std::vector m_rgbRegion; + int dummy; +}; + + +extern "C" +{ + void ColorExtractorInit(RTC::Manager* manager); +}; + +#endif // COLOR_EXTRACTOR_H diff --git a/rtc/ColorExtractor/ColorExtractor.txt b/rtc/ColorExtractor/ColorExtractor.txt new file mode 100644 index 00000000000..933e2c6b6d9 --- /dev/null +++ b/rtc/ColorExtractor/ColorExtractor.txt @@ -0,0 +1,53 @@ +/** + +\page ColorExtractor + +\section introduction Overview + +This component extracts pixels in the given RGB region and computes centroid of them. + + + + +
    implementation_idColorExtractor
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + +
    port namedata typeunitdescription
    originalImg::TimedCameraImagesource image
    + +\subsection outports Output Ports + + + + + +
    port namedata typeunitdescription
    resultImg::TimedCameraImageresult image. The centroid of extracted pixels is shown by a circle.
    posRTC::TimedPoint2DThe centroid of extracted pixels.
    + +\section serviceports Service Ports + +\subsection provider Service Providers + +N/A + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + + + +
    nametypeunitdefault valuedescription
    minPixelsint0the minimum pixels to get an output
    rgbRegionstd::vector0,0,0,0,0,0A region of RGB to extract. R_min, G_min, B_min, R_max, G_max and B_max
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/ColorExtractor/ColorExtractorComp.cpp b/rtc/ColorExtractor/ColorExtractorComp.cpp new file mode 100644 index 00000000000..0226ffc91f9 --- /dev/null +++ b/rtc/ColorExtractor/ColorExtractorComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file ColorExtractorComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "ColorExtractor.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + ColorExtractorInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("ColorExtractor"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/ColorExtractor/test.py b/rtc/ColorExtractor/test.py new file mode 100644 index 00000000000..cedc794c052 --- /dev/null +++ b/rtc/ColorExtractor/test.py @@ -0,0 +1,30 @@ +import rtm + +rtm.nshost="localhost" +rtm.nsport=2809 +rtm.initCORBA() + +mgr = rtm.findRTCmanager() + +mgr.load("CameraImageLoader") +loader = mgr.create("CameraImageLoader") + +mgr.load("ColorExtractor") +extractor = mgr.create("ColorExtractor") +extractor.setProperty("minPixels","10") +extractor.setProperty("rgbRegion","100,0,0,255,100,100") + +mgr.load("CameraImageViewer") +viewer = mgr.create("CameraImageViewer") + +#rtm.serializeComponents([loader, extractor, viewer]) + +rtm.connectPorts(loader.port("image"), extractor.port("original")) +rtm.connectPorts(extractor.port("result"), viewer.port("imageIn")) + +viewer.start() +extractor.start() +loader.start() + + + diff --git a/rtc/DataLogger/CMakeLists.txt b/rtc/DataLogger/CMakeLists.txt index a19b81210b2..e760195db16 100644 --- a/rtc/DataLogger/CMakeLists.txt +++ b/rtc/DataLogger/CMakeLists.txt @@ -7,19 +7,27 @@ set_target_properties(DataLogger PROPERTIES PREFIX "") add_executable(DataLoggerComp DataLoggerComp.cpp ${comp_sources}) target_link_libraries(DataLoggerComp ${libs}) -find_package(PCL) -if (PCL_FOUND AND "${PCL_VERSION_MINOR}" GREATER 6) +add_executable(logSplitter logSplitter.cpp) +target_link_libraries(logSplitter ${libs}) + +find_package(VTK 5.8 EXACT) +if (NOT "${VTK_LIBRARIES}" STREQUAL "") + list(REMOVE_ITEM VTK_LIBRARIES "vtkproj4") +endif() + +if ( PCL_FOUND AND PCL_IO_FOUND AND VTK_FOUND AND "${PCL_VERSION_MINOR}" GREATER 6) + include(${VTK_USE_FILE}) include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_executable(PointCloudLogViewer PointCloudLogViewer) - target_link_libraries(PointCloudLogViewer ${PCL_LIBRARIES}) + target_link_libraries(PointCloudLogViewer ${PCL_LIBRARIES} ${VTK_LIBRARIES}) set(target DataLogger DataLoggerComp PointCloudLogViewer) else() set(target DataLogger DataLoggerComp) endif() install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/DataLogger/DataLogger.cpp b/rtc/DataLogger/DataLogger.cpp index 7142f9e9967..df120b3cc55 100644 --- a/rtc/DataLogger/DataLogger.cpp +++ b/rtc/DataLogger/DataLogger.cpp @@ -7,9 +7,10 @@ * $Id$ */ +#include "hrpsys/util/Hrpsys.h" +#include "hrpsys/idl/pointcloud.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include "DataLogger.h" -#include "util/Hrpsys.h" -#include "pointcloud.hh" typedef coil::Guard Guard; @@ -29,44 +30,74 @@ static const char* nullcomponent_spec[] = "language", "C++", "lang_type", "compile", // Configuration variables + "conf.default.log_precision", "0", "" }; //     +#define LOG_SET_PRECISION(strm) \ + int prc; \ + if (precision != 0) { \ + prc = os.precision(); \ + os << std::scientific << std::setprecision(precision); \ + } \ -void printData(std::ostream& os, const RTC::Acceleration3D& data) +#define LOG_UNSET_PRECISION(strm) \ + if (precision != 0) \ + os << std::fixed << std::setprecision(prc); \ + +void printData(std::ostream& os, const RTC::Acceleration3D& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); os << data.ax << " " << data.ay << " " << data.az << " "; + LOG_UNSET_PRECISION(os); } -void printData(std::ostream& os, const RTC::Velocity2D& data) +void printData(std::ostream& os, const RTC::Velocity2D& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); os << data.vx << " " << data.vy << " " << data.va << " "; + LOG_UNSET_PRECISION(os); } -void printData(std::ostream& os, const RTC::Pose3D& data) +void printData(std::ostream& os, const RTC::Pose3D& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); os << data.position.x << " " << data.position.y << " " << data.position.z << " " << data.orientation.r << " " << data.orientation.p << " " << data.orientation.y << " "; + LOG_UNSET_PRECISION(os); } -void printData(std::ostream& os, const RTC::AngularVelocity3D& data) +void printData(std::ostream& os, const RTC::AngularVelocity3D& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); os << data.avx << " " << data.avy << " " << data.avz << " "; + LOG_UNSET_PRECISION(os); +} + +void printData(std::ostream& os, const RTC::Point3D& data, unsigned int precision = 0) +{ + LOG_SET_PRECISION(os); + os << data.x << " " << data.y << " " << data.z << " "; + LOG_UNSET_PRECISION(os); } -void printData(std::ostream& os, const RTC::Point3D& data) +void printData(std::ostream& os, const RTC::Vector3D& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); os << data.x << " " << data.y << " " << data.z << " "; + LOG_UNSET_PRECISION(os); } -void printData(std::ostream& os, const RTC::Orientation3D& data) +void printData(std::ostream& os, const RTC::Orientation3D& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); os << data.r << " " << data.p << " " << data.y << " "; + LOG_UNSET_PRECISION(os); } -void printData(std::ostream& os, const PointCloudTypes::PointCloud& data) +void printData(std::ostream& os, const PointCloudTypes::PointCloud& data, unsigned int precision = 0) { uint npoint = data.data.length()/data.point_step; os << data.width << " " << data.height << " " << data.type << " " << npoint; @@ -96,12 +127,58 @@ std::ostream& operator<<(std::ostream& os, const _CORBA_Unbounded_Sequence & return os; } +std::ostream& operator<<(std::ostream& os, const OpenHRP::RobotHardwareService::DblSequence6 & data) +{ + for (unsigned int j=0; j -void printData(std::ostream& os, const T& data) +void printData(std::ostream& os, const T& data, unsigned int precision = 0) { + LOG_SET_PRECISION(os); for (unsigned int j=0; j @@ -112,16 +189,18 @@ class LoggerPort : public LoggerPortBase const char *name(){ return m_port.name(); } - virtual void dumpLog(std::ostream& os){ + virtual void dumpLog(std::ostream& os, unsigned int precision = 0){ os.setf(std::ios::fixed, std::ios::floatfield); for (unsigned int i=0; i& port(){ return m_port; } @@ -147,19 +226,18 @@ class LoggerPortForPointCloud : public LoggerPort { public: LoggerPortForPointCloud(const char *name) : LoggerPort(name) {} - void dumpLog(std::ostream& os){ + void dumpLog(std::ostream& os, unsigned int precision = 0){ os.setf(std::ios::fixed, std::ios::floatfield); for (unsigned int i=0; i @@ -167,6 +245,7 @@ DataLogger::DataLogger(RTC::Manager* manager) m_DataLoggerServicePort("DataLoggerService"), //     m_suspendFlag(false), + m_log_precision(0), dummy(0) { m_service0.setLogger(this); @@ -181,6 +260,8 @@ DataLogger::~DataLogger() RTC::ReturnCode_t DataLogger::onInitialize() { std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl; + bindParameter("log_precision", m_log_precision, "0"); + // Registration: InPort/OutPort/Service // // Set InPort buffers @@ -352,6 +433,13 @@ bool DataLogger::add(const char *i_type, const char *i_name) resumeLogging(); return false; } + }else if (strcmp(i_type, "TimedVector3D")==0){ + LoggerPort *lp = new LoggerPort(i_name); + new_port = lp; + if (!addInPort(i_name, lp->port())) { + resumeLogging(); + return false; + } }else if (strcmp(i_type, "TimedOrientation3D")==0){ LoggerPort *lp = new LoggerPort(i_name); new_port = lp; @@ -394,6 +482,13 @@ bool DataLogger::add(const char *i_type, const char *i_name) resumeLogging(); return false; } + }else if (strcmp(i_type, "TimedRobotState2")==0){ + LoggerPort *lp = new LoggerPort(i_name); + new_port = lp; + if (!addInPort(i_name, lp->port())) { + resumeLogging(); + return false; + } }else{ std::cout << "DataLogger: unsupported data type(" << i_type << ")" << std::endl; @@ -415,7 +510,7 @@ bool DataLogger::save(const char *i_basename) fname.append(m_ports[i]->name()); std::ofstream ofs(fname.c_str()); if (ofs.is_open()){ - m_ports[i]->dumpLog(ofs); + m_ports[i]->dumpLog(ofs, m_log_precision); }else{ std::cerr << "[" << m_profile.instance_name << "] failed to open(" << fname << ")" << std::endl; ret = false; diff --git a/rtc/DataLogger/DataLogger.h b/rtc/DataLogger/DataLogger.h index d59f0404afa..c6dd777422f 100644 --- a/rtc/DataLogger/DataLogger.h +++ b/rtc/DataLogger/DataLogger.h @@ -13,6 +13,9 @@ #include #include +#include +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include @@ -20,7 +23,6 @@ #include #include #include -#include "HRPDataTypes.hh" // Service implementation headers // @@ -43,7 +45,7 @@ class LoggerPortBase LoggerPortBase() : m_maxLength(DEFAULT_MAX_LOG_LENGTH) {} virtual const char *name() = 0; virtual void clear() = 0; - virtual void dumpLog(std::ostream& os) = 0; + virtual void dumpLog(std::ostream& os, unsigned int precision = 0) = 0; virtual void log() = 0; void maxLength(unsigned int len) { m_maxLength = len; } protected: @@ -162,6 +164,7 @@ class DataLogger private: bool m_suspendFlag; coil::Mutex m_suspendFlagMutex; + unsigned int m_log_precision; int dummy; }; diff --git a/rtc/DataLogger/DataLoggerService_impl.h b/rtc/DataLogger/DataLoggerService_impl.h index 2fc199022c2..235cfc50987 100644 --- a/rtc/DataLogger/DataLoggerService_impl.h +++ b/rtc/DataLogger/DataLoggerService_impl.h @@ -2,7 +2,7 @@ #ifndef __DATA_LOGGER_SERVICE_IMPL_H__ #define __DATA_LOGGER_SERVICE_IMPL_H__ -#include "DataLoggerService.hh" +#include "hrpsys/idl/DataLoggerService.hh" using namespace OpenHRP; diff --git a/rtc/DataLogger/logSplitter.cpp b/rtc/DataLogger/logSplitter.cpp new file mode 100644 index 00000000000..7b6ab2013f9 --- /dev/null +++ b/rtc/DataLogger/logSplitter.cpp @@ -0,0 +1,129 @@ +#include +#include +#include +#include +#include +#include "hrpsys/idl/RobotHardwareService.hh" + +int main(int argc, char *argv[]) +{ + if (argc < 9){ + std::cerr << "Usage: " << argv[0] << " [basename of rstate2 log] [dof] [no. of extra servo states] [no. of force sensors] [no. of 3 axes gyro] [no. of 3 axes accelerometers] [no. of batteries] [no. of thermometers]" << std::endl; + return 1; + } + + std::string basename(argv[1]); + std::ifstream ifs((basename+".rstate2").c_str()); + if (!ifs.is_open()){ + std::cerr << "failed to open " << argv[1] << ".rstate2" << std::endl; + return 2; + } + int dof = atoi(argv[2]); + int nextrass = atoi(argv[3]); + int nfsensor = atoi(argv[4]); + int ngyro = atoi(argv[5]); + int naccel = atoi(argv[6]); + int nbattery = atoi(argv[7]); + int ntemp = atoi(argv[8]); + + std::ofstream ofsq((basename+".q").c_str()); + std::ofstream ofsqref((basename+".qRef").c_str()); + std::ofstream ofstau((basename+".tau").c_str()); + std::ofstream ofsss((basename+".sstate").c_str()); + std::ofstream ofsfsensor((basename+".fsensor").c_str()); + std::ofstream ofsgyro((basename+".gyro").c_str()); + std::ofstream ofsaccel((basename+".accel").c_str()); + std::ofstream ofsbat((basename+".bat").c_str()); + std::ofstream ofstemp((basename+".temp").c_str()); + + ofsq.setf(std::ios::fixed, std::ios::floatfield); + ofsqref.setf(std::ios::fixed, std::ios::floatfield); + ofstau.setf(std::ios::fixed, std::ios::floatfield); + ofsss.setf(std::ios::fixed, std::ios::floatfield); + ofsfsensor.setf(std::ios::fixed, std::ios::floatfield); + ofsgyro.setf(std::ios::fixed, std::ios::floatfield); + ofsaccel.setf(std::ios::fixed, std::ios::floatfield); + ofsbat.setf(std::ios::fixed, std::ios::floatfield); + ofstemp.setf(std::ios::fixed, std::ios::floatfield); + + double time, v; + int ss; + std::string str; + + ifs >> time; + while(!ifs.eof()){ + // q + ofsq << time << " "; + for (int i=0; i> v; ofsq << v << " "; + } + ofsq << std::endl; + // qRef + ofsqref << time << " "; + for (int i=0; i> v; ofsqref << v << " "; + } + ofsqref << std::endl; + // tau + ofstau << time << " "; + for (int i=0; i> v; ofstau << v << " "; + } + ofstau << std::endl; + // servo state + ofsss << time << " "; + for (int i=0; i> ss; + ofsss << ((ss&OpenHRP::RobotHardwareService::CALIB_STATE_MASK) >> OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT) << " "; + ofsss << ((ss&OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT) << " "; + ofsss << ((ss&OpenHRP::RobotHardwareService::POWER_STATE_MASK) >> OpenHRP::RobotHardwareService::POWER_STATE_SHIFT) << " "; + ofsss << ((ss&OpenHRP::RobotHardwareService::SERVO_ALARM_MASK) >> OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT) << " "; + ofsss << ((ss&OpenHRP::RobotHardwareService::DRIVER_TEMP_MASK) >> OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT) << " "; + for (int j=0; j> ss; + ofsss << ss << " "; + } + } + ofsss << std::endl; + // force sensor + ofsfsensor << time << " "; + for (int i=0; i<6*nfsensor; i++){ + ifs >> v; ofsfsensor << v << " "; + } + ofsfsensor << std::endl; + // gyro + ofsgyro << time << " "; + for (int i=0; i<3*ngyro; i++){ + ifs >> v; ofsgyro << v << " "; + } + ofsgyro << std::endl; + // accelerometer + ofsaccel << time << " "; + for (int i=0; i<3*naccel; i++){ + ifs >> v; ofsaccel << v << " "; + } + ofsaccel << std::endl; + // battery + ofsbat << time << " "; + for (int i=0; i<3*nbattery+2; i++){ + ifs >> str; + if (str == "nan"){ + v = std::numeric_limits::quiet_NaN(); + }else{ + v = atof(str.c_str()); + } + ofsbat << v << " "; + } + ofsbat << std::endl; + // thermometer + ofstemp << time << " "; + for (int i=0; i> v; ofstemp << v << " "; + } + ofstemp << std::endl; + + ifs >> time; + } + + return 0; +} diff --git a/rtc/EmergencyStopper/CMakeLists.txt b/rtc/EmergencyStopper/CMakeLists.txt index d5056f9f57d..b3803ab8313 100644 --- a/rtc/EmergencyStopper/CMakeLists.txt +++ b/rtc/EmergencyStopper/CMakeLists.txt @@ -12,6 +12,6 @@ target_link_libraries(EmergencyStopperComp ${libs}) set(target EmergencyStopper EmergencyStopperComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/EmergencyStopper/EmergencyStopper.cpp b/rtc/EmergencyStopper/EmergencyStopper.cpp index 0dd5b4df4d0..18a9fe9344f 100644 --- a/rtc/EmergencyStopper/EmergencyStopper.cpp +++ b/rtc/EmergencyStopper/EmergencyStopper.cpp @@ -7,19 +7,26 @@ * $Id$ */ -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include #include #include #include #include -#include "RobotHardwareService.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include "EmergencyStopper.h" -#include "../SoftErrorLimiter/beep.h" +#include typedef coil::Guard Guard; +#ifndef rad2deg +#define rad2deg(rad) (rad * 180 / M_PI) +#endif +#ifndef deg2rad +#define deg2rad(deg) (deg * M_PI / 180) +#endif + // Module specification // static const char* emergencystopper_spec[] = @@ -40,30 +47,43 @@ static const char* emergencystopper_spec[] = }; // +static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm) +{ + int pre = os.precision(); + os.setf(std::ios::fixed); + os << std::setprecision(6) + << (tm.sec + tm.nsec/1e9) + << std::setprecision(pre); + os.unsetf(std::ios::fixed); + return os; +} + EmergencyStopper::EmergencyStopper(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // m_qRefIn("qRef", m_qRef), + m_qEmergencyIn("qEmergency", m_qEmergency), m_emergencySignalIn("emergencySignal", m_emergencySignal), + m_emergencyFallMotionIn("emergencyFallMotion", m_emergencyFallMotion), + m_servoStateIn("servoStateIn", m_servoState), m_qOut("q", m_q), + m_qTouchWallOut("qTouchWall", m_qTouchWall), m_emergencyModeOut("emergencyMode", m_emergencyMode), + m_beepCommandOut("beepCommand", m_beepCommand), + m_touchWallMotionSolvedOut("touchWallMotionSolved", m_touchWallMotionSolved), m_EmergencyStopperServicePort("EmergencyStopperService"), - m_servoStateIn("servoStateIn", m_servoState), // m_robot(hrp::BodyPtr()), m_debugLevel(0), - dummy(0), loop(0), - emergency_stopper_beep_count(0) + emergency_stopper_beep_count(0), + dummy(0) { m_service0.emergencystopper(this); - init_beep(); - start_beep(3136); } EmergencyStopper::~EmergencyStopper() { - quit_beep(); } @@ -79,12 +99,17 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize() // // Set InPort buffers addInPort("qRef", m_qRefIn); + addInPort("qEmergency", m_qEmergencyIn); addInPort("emergencySignal", m_emergencySignalIn); + addInPort("emergencyFallMotion", m_emergencyFallMotionIn); addInPort("servoStateIn", m_servoStateIn); // Set OutPort buffer addOutPort("q", m_qOut); + addOutPort("qTouchWall", m_qTouchWallOut); addOutPort("emergencyMode", m_emergencyModeOut); + addOutPort("beepCommand", m_beepCommandOut); + addOutPort("touchWallMotionSolved", m_touchWallMotionSolvedOut); // Set service provider to Ports m_EmergencyStopperServicePort.registerProvider("service0", "EmergencyStopperService", m_service0); @@ -126,23 +151,23 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize() OpenHRP::LinkInfoSequence_var lis = binfo->links(); std::vector fsensor_names; // find names for real force sensors - for ( int k = 0; k < lis->length(); k++ ) { + for ( unsigned int k = 0; k < lis->length(); k++ ) { OpenHRP::SensorInfoSequence& sensors = lis[k].sensors; - for ( int l = 0; l < sensors.length(); l++ ) { + for ( unsigned int l = 0; l < sensors.length(); l++ ) { if ( std::string(sensors[l].type) == "Force" ) { fsensor_names.push_back(std::string(sensors[l].name)); } } } - int npforce = fsensor_names.size(); + unsigned int npforce = fsensor_names.size(); // find names for virtual force sensors coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ","); - int nvforce = virtual_force_sensor.size()/10; + unsigned int nvforce = virtual_force_sensor.size()/10; for (unsigned int i=0; inumJoints()]; + m_stop_posture.resize(m_robot->numJoints(), 0.0); + m_motion_posture.resize(m_robot->numJoints(), 0.0); m_stop_wrenches = new double[nforce*6]; m_tmp_wrenches = new double[nforce*6]; m_interpolator = new interpolator(m_robot->numJoints(), recover_time_dt); @@ -178,11 +208,11 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize() m_wrenches_interpolator->setName(std::string(m_profile.instance_name)+" interpolator wrenches"); m_q.data.length(m_robot->numJoints()); - for(int i=0; inumJoints(); i++){ + for(unsigned int i=0; inumJoints(); i++){ m_q.data[i] = 0; - m_stop_posture[i] = 0; } - for(int i=0; inumJoints()); + for(unsigned int i=0; inumJoints()); - for(int i = 0; i < m_robot->numJoints(); i++) { + for(unsigned int i = 0; i < m_robot->numJoints(); i++) { m_servoState.data[i].length(1); int status = 0; status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT; @@ -202,6 +232,7 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize() } emergency_stopper_beep_freq = static_cast(1.0/(2.0*m_dt)); // 2 times / 1[s] + m_beepCommand.data.length(bc.get_num_beep_info()); return RTC::RTC_OK; } @@ -212,7 +243,6 @@ RTC::ReturnCode_t EmergencyStopper::onFinalize() { delete m_interpolator; delete m_wrenches_interpolator; - delete m_stop_posture; delete m_stop_wrenches; delete m_tmp_wrenches; return RTC::RTC_OK; @@ -244,6 +274,9 @@ RTC::ReturnCode_t EmergencyStopper::onDeactivated(RTC::UniqueId ec_id) Guard guard(m_mutex); if (is_stop_mode) { is_stop_mode = false; + emergency_mode = 0; + is_emergency_fall_motion = false; + solved = false; recover_time = 0; m_interpolator->setGoal(m_qRef.data.get_buffer(), m_dt); m_interpolator->get(m_q.data.get_buffer()); @@ -253,7 +286,7 @@ RTC::ReturnCode_t EmergencyStopper::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) { - int numJoints = m_robot->numJoints(); + unsigned int numJoints = m_robot->numJoints(); loop++; if (m_servoStateIn.isNew()) { m_servoStateIn.read(); @@ -273,19 +306,37 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) m_qRefIn.read(); assert(m_qRef.data.length() == numJoints); std::vector current_posture; - for ( int i = 0; i < m_qRef.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) { current_posture.push_back(m_qRef.data[i]); } m_input_posture_queue.push(current_posture); - while (m_input_posture_queue.size() > default_retrieve_time) { + while ((int)m_input_posture_queue.size() > default_retrieve_time) { m_input_posture_queue.pop(); } if (!is_stop_mode) { - for ( int i = 0; i < m_qRef.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) { if (recover_time > 0) { // Until releasing is finished, do not use m_stop_posture in input queue because too large error. m_stop_posture[i] = m_q.data[i]; } else { - m_stop_posture[i] = m_input_posture_queue.front()[i]; + m_stop_posture[i] = m_input_posture_queue.front()[i]; + } + } + } + { + // double tmpq[] = {-6.827925e-08, -3.735005e-06, -0.929562, 2.46032, -1.53045, 3.839724e-06, 6.827917e-08, 3.735005e-06, -0.929564, 2.46032, -1.53045, -3.839724e-06}; // CHIDORI + double tmpq[] = {7.164374e-09, -1.013475e-07, -1.00249, 1.6208, -0.618309, 1.068601e-07, -3.914983e-10, 2.927592e-08, -0.82775, 1.6555, -0.827748, -3.091128e-08, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.39626, -0.349066, -0.087266, 0.0, 0.0, 0.0, -0.349066, 0.0, -1.39626, 0.349066, 0.087266, 0.0, 0.0, 0.0, -0.349066, -1.39626, 1.39626, -1.39626, 1.39626}; // JAXON + // double tmpq[] = { + // // -0.031978, -0.302196, 0.052625, 0.623091, -0.052851, -0.319949, -0.000000, -0.000291, -0.032075, -0.302298, 0.052653, 0.623232, -0.052929, -0.320630, -0.000000, -0.000063, 0.261282, 0.869966, -0.087213, -0.000439, 0.000020, -0.000080, 0.000013, 0.890118, 0.261531, -0.086439, 0.087310, -0.523310, -0.000004, 0.000032, -0.000003, 0.890118, 0.298616, 0.298616, 0.259056, 0.294777, 0.294777, 0.298616, 0.298616, 0.259056, 0.294777, 0.294777, 0.311048, 0.311048, 0.221701, 0.221701 // stretch larm + // 0.000005, -0.348759, 0.000012, 0.630766, 0.000030, -0.278725, 0.000000, -0.000211, -0.000005, -0.348793, 0.000024, 0.630764, 0.000044, -0.278784, -0.000000, -0.000065, 0.261541, 0.086439, -0.087310, -0.523306, 0.000004, 0.000033, 0.000003, 0.890118, 0.261541, -0.086438, 0.087310, -0.523306, -0.000004, 0.000033, -0.000003, 0.890118, 0.298616, 0.298616, 0.259056, 0.294777, 0.294777, 0.298616, 0.298616, 0.259056, 0.294777, 0.294777, 0.311048, 0.311048, 0.221701, 0.221701 // reset-pose + // }; // RHP4B + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ) { + if (recover_time > 0 && !is_stop_mode) { // Until releasing is finished, do not use m_stop_posture in input queue because too large error. + m_motion_posture[i] = m_q.data[i]; + } else { + if (!solved) { + if (sizeof(tmpq)/sizeof(double) == m_qRef.data.length()) m_motion_posture[i] = tmpq[i]; + else m_motion_posture[i] = m_q.data[i]; + } else m_motion_posture[i] = m_qEmergency.data[i]; } } } @@ -296,17 +347,17 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) } } std::vector current_wrench; - for ( int i= 0; i < m_wrenchesRef.size(); i++ ) { + for ( unsigned int i= 0; i < m_wrenchesRef.size(); i++ ) { for (int j = 0; j < 6; j++ ) { current_wrench.push_back(m_wrenchesRef[i].data[j]); } } m_input_wrenches_queue.push(current_wrench); - while (m_input_wrenches_queue.size() > default_retrieve_time) { + while ((int)m_input_wrenches_queue.size() > default_retrieve_time) { m_input_wrenches_queue.pop(); } if (!is_stop_mode) { - for ( int i= 0; i < m_wrenchesRef.size(); i++ ) { + for ( unsigned int i= 0; i < m_wrenchesRef.size(); i++ ) { for (int j = 0; j < 6; j++ ) { if (recover_time > 0) { m_stop_wrenches[i*6+j] = m_wrenches[i].data[j]; @@ -320,18 +371,53 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) if (m_emergencySignalIn.isNew()){ m_emergencySignalIn.read(); - if ( m_emergencySignal.data == 0 ) { + emergency_mode = m_emergencySignal.data; + if ( emergency_mode == 0 ) { + if (is_stop_mode) { Guard guard(m_mutex); - std::cerr << "[" << m_profile.instance_name << "] emergencySignal is reset!" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] emergencySignal is reset!" << std::endl; is_stop_mode = false; + } } else if (!is_stop_mode) { + Guard guard(m_mutex); + switch (emergency_mode) { + case 1: + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] emergencySignal is set!" << std::endl; + is_stop_mode = true; + break; + case 2: + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] emergencyTouchWall is set!" << std::endl; + break; + default: + break; + } + } + } + if (m_emergencyFallMotionIn.isNew()) { + m_emergencyFallMotionIn.read(); + if ( !m_emergencyFallMotion.data ) { + Guard guard(m_mutex); + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] emergencyFallMotion is reset!" << std::endl; + is_stop_mode = false; + emergency_mode = 0; + is_emergency_fall_motion = false; + } else { Guard guard(m_mutex); - std::cerr << "[" << m_profile.instance_name << "] emergencySignal is set!" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] emergencyFallMotion is set!" << std::endl; is_stop_mode = true; + is_emergency_fall_motion = true; } } + if (m_qEmergencyIn.isNew()) { + m_qEmergencyIn.read(); + } if (is_stop_mode && !prev_is_stop_mode) { - retrieve_time = default_retrieve_time; + retrieve_time = default_retrieve_duration; // Reflect current output joint angles to interpolator state m_interpolator->set(m_q.data.get_buffer()); get_wrenches_array_from_data(m_wrenches, m_tmp_wrenches); @@ -356,10 +442,10 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) m_wrenches_interpolator->get(m_tmp_wrenches); set_wrenches_data_from_array(m_wrenches, m_tmp_wrenches); } else { - for ( int i = 0; i < m_q.data.length(); i++ ) { + for ( unsigned int i = 0; i < m_q.data.length(); i++ ) { m_q.data[i] = m_qRef.data[i]; } - for ( int i = 0; i < m_wrenches.size(); i++ ) { + for ( unsigned int i = 0; i < m_wrenches.size(); i++ ) { for ( int j = 0; j < 6; j++ ) { m_wrenches[i].data[j] = m_wrenchesRef[i].data[j]; } @@ -369,7 +455,8 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) recover_time = default_recover_time; if (retrieve_time > 0 ) { retrieve_time = retrieve_time - recover_time_dt; - m_interpolator->setGoal(m_stop_posture, retrieve_time); + if (is_emergency_fall_motion) m_interpolator->setGoal(m_motion_posture.data(), retrieve_time); + else m_interpolator->setGoal(m_stop_posture.data(), retrieve_time); m_interpolator->get(m_q.data.get_buffer()); m_wrenches_interpolator->setGoal(m_stop_wrenches, retrieve_time); m_wrenches_interpolator->get(m_tmp_wrenches); @@ -382,12 +469,12 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) // write data if (DEBUGP) { std::cerr << "q: "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << m_q.data[i] ; } std::cerr << std::endl; std::cerr << "wrenches: "; - for (int i = 0; i < m_wrenches.size(); i++) { + for (unsigned int i = 0; i < m_wrenches.size(); i++) { for (int j = 0; j < 6; j++ ) { std::cerr << " " << m_wrenches[i].data[j]; } @@ -403,27 +490,37 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id) m_wrenchesOut[i]->write(); } - m_emergencyMode.data = is_stop_mode; + m_emergencyMode.data = emergency_mode; m_emergencyMode.tm = m_qRef.tm; m_emergencyModeOut.write(); + m_touchWallMotionSolved.data = solved; + m_touchWallMotionSolved.tm = m_qRef.tm; + m_touchWallMotionSolvedOut.write(); prev_is_stop_mode = is_stop_mode; // beep sound for emergency stop alert // check servo for emergency stop beep sound bool has_servoOn = false; - for (int i = 0; i < m_robot->numJoints(); i++ ){ + for (unsigned int i = 0; i < m_robot->numJoints(); i++ ){ int servo_state = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; has_servoOn = has_servoOn || (servo_state == 1); } // beep if ( is_stop_mode && has_servoOn ) { // If stop mode and some joint is servoOn - if ( emergency_stopper_beep_count % emergency_stopper_beep_freq == 0 && emergency_stopper_beep_count % (emergency_stopper_beep_freq * 3) != 0 ) start_beep(2352, emergency_stopper_beep_freq*0.7); - else stop_beep(); + if ( emergency_stopper_beep_count % emergency_stopper_beep_freq == 0 && emergency_stopper_beep_count % (emergency_stopper_beep_freq * 3) != 0 ) { + bc.startBeep(2352, emergency_stopper_beep_freq*0.7); + } else { + bc.stopBeep(); + } emergency_stopper_beep_count++; } else { emergency_stopper_beep_count = 0; + bc.stopBeep(); } + bc.setDataPort(m_beepCommand); + m_beepCommand.tm = m_qRef.tm; + if (bc.isWritable()) m_beepCommandOut.write(); return RTC::RTC_OK; } @@ -475,8 +572,11 @@ bool EmergencyStopper::stopMotion() bool EmergencyStopper::releaseMotion() { Guard guard(m_mutex); - if (is_stop_mode) { + { is_stop_mode = false; + emergency_mode = 0; + is_emergency_fall_motion = false; + solved = false; std::cerr << "[" << m_profile.instance_name << "] releaseMotion is called" << std::endl; } return true; @@ -487,6 +587,7 @@ bool EmergencyStopper::getEmergencyStopperParam(OpenHRP::EmergencyStopperService std::cerr << "[" << m_profile.instance_name << "] getEmergencyStopperParam" << std::endl; i_param.default_recover_time = default_recover_time*m_dt; i_param.default_retrieve_time = default_retrieve_time*m_dt; + i_param.default_retrieve_duration = default_retrieve_duration*m_dt; i_param.is_stop_mode = is_stop_mode; return true; }; @@ -496,10 +597,29 @@ bool EmergencyStopper::setEmergencyStopperParam(const OpenHRP::EmergencyStopperS std::cerr << "[" << m_profile.instance_name << "] setEmergencyStopperParam" << std::endl; default_recover_time = i_param.default_recover_time/m_dt; default_retrieve_time = i_param.default_retrieve_time/m_dt; - std::cerr << "[" << m_profile.instance_name << "] default_recover_time = " << default_recover_time*m_dt << "[s], default_retrieve_time = " << default_retrieve_time*m_dt << "[s]" << std::endl; + default_retrieve_duration = i_param.default_retrieve_duration/m_dt; + std::cerr << "[" << m_profile.instance_name << "] default_recover_time = " << default_recover_time*m_dt << "[s], default_retrieve_time = " << default_retrieve_time*m_dt << "[s], default_retrieve_duration = " << default_retrieve_duration*m_dt << "[s]" << std::endl; return true; }; +bool EmergencyStopper::setEmergencyJointAngles(const double *angles, const bool _solved) +{ + // interpolate in Autobalancer + for (size_t i = 0; i < m_robot->numJoints(); i++) { + // tmp for jaxon choreonoid + if (i == 33 || i == 35) m_stop_posture[i] = -1.39626; + else if (i == 34 || i == 36) m_stop_posture[i] = 1.39626; + else m_stop_posture[i] = deg2rad(angles[i]); + m_qTouchWall.data[i] = m_stop_posture[i]; + } + retrieve_time = default_retrieve_time; + m_qTouchWall.tm = m_qRef.tm; + m_qTouchWallOut.write(); + solved = _solved; + is_stop_mode = true; + return true; +}; + extern "C" { diff --git a/rtc/EmergencyStopper/EmergencyStopper.h b/rtc/EmergencyStopper/EmergencyStopper.h index 9bc8ab85241..fd666e36e7e 100644 --- a/rtc/EmergencyStopper/EmergencyStopper.h +++ b/rtc/EmergencyStopper/EmergencyStopper.h @@ -10,6 +10,9 @@ #ifndef EMERGENCY_STOPPER_H #define EMERGENCY_STOPPER_H +#include +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include @@ -19,12 +22,12 @@ #include #include #include "interpolator.h" -#include "HRPDataTypes.hh" #include // Service implementation headers // #include "EmergencyStopperService_impl.h" +#include "../SoftErrorLimiter/beep.h" // @@ -103,6 +106,7 @@ class EmergencyStopper bool releaseMotion(); bool getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param); bool setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param); + bool setEmergencyJointAngles(const double *angles, const bool solved); protected: // Configuration variable declaration @@ -111,17 +115,24 @@ class EmergencyStopper // TimedDoubleSeq m_qRef; + TimedDoubleSeq m_qEmergency; TimedDoubleSeq m_q; + TimedDoubleSeq m_qTouchWall; TimedLong m_emergencySignal; + TimedBoolean m_emergencyFallMotion; TimedLong m_emergencyMode; OpenHRP::TimedLongSeqSeq m_servoState; std::vector m_wrenchesRef; std::vector m_wrenches; + TimedLongSeq m_beepCommand; + TimedBoolean m_touchWallMotionSolved; // DataInPort declaration // InPort m_qRefIn; + InPort m_qEmergencyIn; InPort m_emergencySignalIn; + InPort m_emergencyFallMotionIn; InPort m_servoStateIn; std::vector *> m_wrenchesIn; @@ -130,8 +141,11 @@ class EmergencyStopper // DataOutPort declaration // OutPort m_qOut; + OutPort m_qTouchWallOut; OutPort m_emergencyModeOut; std::vector *> m_wrenchesOut; + OutPort m_beepCommandOut; + OutPort m_touchWallMotionSolvedOut; // @@ -154,7 +168,7 @@ class EmergencyStopper private: void get_wrenches_array_from_data(const std::vector &wrenches_data, double *wrenches_array) { - for ( int i= 0; i < wrenches_data.size(); i++ ) { + for ( unsigned int i= 0; i < wrenches_data.size(); i++ ) { for (int j = 0; j < 6; j++ ) { wrenches_array[i*6+j] = wrenches_data[i].data[j]; } @@ -162,7 +176,7 @@ class EmergencyStopper } void set_wrenches_data_from_array(std::vector &wrenches_data, const double *wrenches_array) { - for ( int i= 0; i < wrenches_data.size(); i++ ) { + for ( unsigned int i= 0; i < wrenches_data.size(); i++ ) { for (int j = 0; j < 6; j++ ) { wrenches_data[i].data[j] = wrenches_array[i*6+j]; } @@ -172,13 +186,14 @@ class EmergencyStopper hrp::BodyPtr m_robot; double m_dt; unsigned int m_debugLevel; - int dummy, loop; - bool is_stop_mode, prev_is_stop_mode; + int loop; + bool is_stop_mode, prev_is_stop_mode, is_emergency_fall_motion; bool is_initialized; int recover_time, retrieve_time; double recover_time_dt; - int default_recover_time, default_retrieve_time; - double *m_stop_posture; + int default_recover_time, default_retrieve_time, default_retrieve_duration; + std::vector m_stop_posture; + std::vector m_motion_posture; double *m_stop_wrenches; double *m_tmp_wrenches; interpolator* m_interpolator; @@ -187,6 +202,10 @@ class EmergencyStopper std::queue > m_input_wrenches_queue; int emergency_stopper_beep_count, emergency_stopper_beep_freq; coil::Mutex m_mutex; + BeepClient bc; + int dummy; + bool solved; + int emergency_mode; }; diff --git a/rtc/EmergencyStopper/EmergencyStopperService_impl.cpp b/rtc/EmergencyStopper/EmergencyStopperService_impl.cpp index dd21850b023..8f8c807120c 100644 --- a/rtc/EmergencyStopper/EmergencyStopperService_impl.cpp +++ b/rtc/EmergencyStopper/EmergencyStopperService_impl.cpp @@ -30,6 +30,11 @@ CORBA::Boolean EmergencyStopperService_impl::setEmergencyStopperParam(const Open return m_emergencystopper->setEmergencyStopperParam(i_param); }; +CORBA::Boolean EmergencyStopperService_impl::setEmergencyJointAngles(const OpenHRP::EmergencyStopperService::dSequence& angles, const CORBA::Boolean solved) +{ + return m_emergencystopper->setEmergencyJointAngles(angles.get_buffer(), solved); +}; + void EmergencyStopperService_impl::emergencystopper(EmergencyStopper *i_emergencystopper) { m_emergencystopper = i_emergencystopper; diff --git a/rtc/EmergencyStopper/EmergencyStopperService_impl.h b/rtc/EmergencyStopper/EmergencyStopperService_impl.h index 9832c9a2368..0d74db6e255 100644 --- a/rtc/EmergencyStopper/EmergencyStopperService_impl.h +++ b/rtc/EmergencyStopper/EmergencyStopperService_impl.h @@ -1,7 +1,7 @@ #ifndef __EMERGENCYSTOPPER_SERVICE_H__ #define __EMERGENCYSTOPPER_SERVICE_H__ -#include "EmergencyStopperService.hh" +#include "hrpsys/idl/EmergencyStopperService.hh" class EmergencyStopper; @@ -24,6 +24,7 @@ class EmergencyStopperService_impl void releaseMotion(); CORBA::Boolean getEmergencyStopperParam(OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param); CORBA::Boolean setEmergencyStopperParam(const OpenHRP::EmergencyStopperService::EmergencyStopperParam& i_param); + CORBA::Boolean setEmergencyJointAngles(const OpenHRP::EmergencyStopperService::dSequence& angles, const CORBA::Boolean solved); void emergencystopper(EmergencyStopper *i_emergencystopper); diff --git a/rtc/ExtractCameraImage/CMakeLists.txt b/rtc/ExtractCameraImage/CMakeLists.txt index d43c9e94ff3..b8be3896769 100644 --- a/rtc/ExtractCameraImage/CMakeLists.txt +++ b/rtc/ExtractCameraImage/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(ExtractCameraImageComp ${libs}) set(target ExtractCameraImage ExtractCameraImageComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ExtractCameraImage/ExtractCameraImage.h b/rtc/ExtractCameraImage/ExtractCameraImage.h index ab948b359af..bcb32fb62c7 100644 --- a/rtc/ExtractCameraImage/ExtractCameraImage.h +++ b/rtc/ExtractCameraImage/ExtractCameraImage.h @@ -10,13 +10,14 @@ #ifndef EXTRACT_CAMERA_IMAGE_H #define EXTRACT_CAMERA_IMAGE_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" // Service implementation headers // @@ -133,7 +134,7 @@ class ExtractCameraImage // private: - int m_index; + unsigned int m_index; int dummy; }; diff --git a/rtc/ForwardKinematics/CMakeLists.txt b/rtc/ForwardKinematics/CMakeLists.txt index 1fc8fba1263..998e357fa58 100644 --- a/rtc/ForwardKinematics/CMakeLists.txt +++ b/rtc/ForwardKinematics/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(ForwardKinematicsComp ${libs}) set(target ForwardKinematics ForwardKinematicsComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ForwardKinematics/ForwardKinematics.cpp b/rtc/ForwardKinematics/ForwardKinematics.cpp index 90f883b265b..41be2620b4b 100644 --- a/rtc/ForwardKinematics/ForwardKinematics.cpp +++ b/rtc/ForwardKinematics/ForwardKinematics.cpp @@ -172,7 +172,7 @@ RTC::ReturnCode_t ForwardKinematics::onExecute(RTC::UniqueId ec_id) if (m_qIn.isNew()) { m_qIn.read(); - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ m_actBody->joint(i)->q = m_q.data[i]; } } @@ -197,7 +197,7 @@ RTC::ReturnCode_t ForwardKinematics::onExecute(RTC::UniqueId ec_id) if (m_qRefIn.isNew()) { m_qRefIn.read(); - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ m_refBody->joint(i)->q = m_qRef.data[i]; } } diff --git a/rtc/ForwardKinematics/ForwardKinematics.h b/rtc/ForwardKinematics/ForwardKinematics.h index aa8eb02bce6..4bc11bd7434 100644 --- a/rtc/ForwardKinematics/ForwardKinematics.h +++ b/rtc/ForwardKinematics/ForwardKinematics.h @@ -10,6 +10,8 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H +#include +#include #include #include #include diff --git a/rtc/ForwardKinematics/ForwardKinematicsService_impl.h b/rtc/ForwardKinematics/ForwardKinematicsService_impl.h index 47320fca04f..19b7caf8fc3 100644 --- a/rtc/ForwardKinematics/ForwardKinematicsService_impl.h +++ b/rtc/ForwardKinematics/ForwardKinematicsService_impl.h @@ -1,7 +1,7 @@ #ifndef __FORWARD_KINEMATICS_SERVICE_IMPL_H__ #define __FORWARD_KINEMATICS_SERVICE_IMPL_H__ -#include "ForwardKinematicsService.hh" +#include "hrpsys/idl/ForwardKinematicsService.hh" class ForwardKinematics; diff --git a/rtc/GraspController/CMakeLists.txt b/rtc/GraspController/CMakeLists.txt index 33dd79e8c34..bb825034bfe 100644 --- a/rtc/GraspController/CMakeLists.txt +++ b/rtc/GraspController/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(GraspControllerComp ${libs}) set(target GraspController GraspControllerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/GraspController/GraspController.cpp b/rtc/GraspController/GraspController.cpp index 6e77705b701..b99c3798dac 100644 --- a/rtc/GraspController/GraspController.cpp +++ b/rtc/GraspController/GraspController.cpp @@ -8,10 +8,10 @@ */ #include "GraspController.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include #include -#include "RobotHardwareService.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include @@ -63,7 +63,7 @@ GraspController::~GraspController() RTC::ReturnCode_t GraspController::onInitialize() { - std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + std::cout << "[" << m_profile.instance_name << "] : onInitialize()" << std::endl; // // Bind variables and configuration variable bindParameter("debugLevel", m_debugLevel, "0"); @@ -115,7 +115,8 @@ RTC::ReturnCode_t GraspController::onInitialize() std::string grasp_name; GraspJoint grasp_joint; std::vector grasp_joints; - for(int i = 0, f = 0; i < grasp_joint_params.size(); i++ ){ + std::cerr << "[" << m_profile.instance_name << "] Parse joint group setting..." << std::endl; + for(unsigned int i = 0, f = 0; i < grasp_joint_params.size(); i++ ){ coil::vstring grasp_joint_group_names = coil::split(grasp_joint_params[i], ":"); if ( grasp_joint_group_names.size() > 1 ) { if ( grasp_name != "" ) { @@ -130,9 +131,13 @@ RTC::ReturnCode_t GraspController::onInitialize() grasp_name = grasp_joint_group_names[0]; if ( !! m_robot->link(grasp_joint_group_names[1]) ) { grasp_joint.id = m_robot->link(std::string(grasp_joint_group_names[1].c_str()))->jointId; + } else { + std::cerr << "[" << m_profile.instance_name << "] No such grasp joint name " << grasp_joint_group_names[1] << std::endl; } f = 0; i++; + } else { + std::cerr << "[" << m_profile.instance_name << "] Invalid joint group setting (length " << grasp_joint_group_names.size() << " should be > 1" << std::endl; } if ( f == 0 ) { coil::stringTo(grasp_joint.dir,grasp_joint_params[i].c_str()); @@ -141,6 +146,8 @@ RTC::ReturnCode_t GraspController::onInitialize() } else { if ( !! m_robot->link(grasp_joint_params[i]) ) { grasp_joint.id = m_robot->link(grasp_joint_params[i])->jointId; + } else { + std::cerr << "[" << m_profile.instance_name << "] No such grasp joint name " << grasp_joint_params[i] << std::endl; } f = 0 ; } @@ -154,16 +161,15 @@ RTC::ReturnCode_t GraspController::onInitialize() m_grasp_param[grasp_name] = grasp_param; } // - if ( m_debugLevel ) { - std::map::iterator it = m_grasp_param.begin(); - while ( it != m_grasp_param.end() ) { - std::cerr << "[" << m_profile.instance_name << "] " << it->first << " : "; - for ( int i = 0 ; i < it->second.joints.size(); i++ ) { + std::cerr << "[" << m_profile.instance_name << "] Joint group setting results." << std::endl; + std::map::iterator it = m_grasp_param.begin(); + while ( it != m_grasp_param.end() ) { + std::cerr << "[" << m_profile.instance_name << "] " << it->first << " : "; + for ( unsigned int i = 0 ; i < it->second.joints.size(); i++ ) { std::cerr << "id = " << it->second.joints[i].id << ", dir = " << it->second.joints[i].dir << ", "; } std::cerr << std::endl; it++; - } } @@ -193,13 +199,13 @@ RTC::ReturnCode_t GraspController::onShutdown(RTC::UniqueId ec_id) RTC::ReturnCode_t GraspController::onActivated(RTC::UniqueId ec_id) { - std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + std::cout << "[" << m_profile.instance_name<< "] : onActivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; } RTC::ReturnCode_t GraspController::onDeactivated(RTC::UniqueId ec_id) { - std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + std::cout << "[" << m_profile.instance_name<< "] : onDeactivated(" << ec_id << ")" << std::endl; for (std::map::iterator it = m_grasp_param.begin(); it != m_grasp_param.end(); it++ ) { it->second.time = 2; // count down to 1 it->second.target_error = 0; @@ -230,9 +236,9 @@ RTC::ReturnCode_t GraspController::onExecute(RTC::UniqueId ec_id) grasp_param.time++; } else if ( grasp_param.time == 0 ) {// working //std::cerr << "grasp mode " << std::endl; - for ( int j= 0; j < grasp_param.joints.size(); j++ ) { + for ( unsigned int j= 0; j < grasp_param.joints.size(); j++ ) { int i = grasp_param.joints[j].id; - if ( 0 <= i && i < m_qRef.data.length() ) { + if ( 0 <= i && (unsigned int)i < m_qRef.data.length() ) { double error = (m_qCurrent.data[i] - m_qRef.data[i]) + grasp_param.target_error * grasp_param.joints[j].dir; double diff = fabs(error); if ( error > 0 ) m_q.data[i] = m_qRef.data[i] + diff; @@ -244,9 +250,9 @@ RTC::ReturnCode_t GraspController::onExecute(RTC::UniqueId ec_id) } } else if ( grasp_param.time > 1 ) { // stopping grasp_param.time--; - for ( int j= 0; j < grasp_param.joints.size(); j++ ) { + for ( unsigned int j= 0; j < grasp_param.joints.size(); j++ ) { int i = grasp_param.joints[j].id; - if ( 0 <= i && i < m_qRef.data.length() ) { + if ( 0 <= i && (unsigned int)i < m_qRef.data.length() ) { m_qRef.data[i] = (m_qRef.data[i] - m_q.data[i] ) * grasp_param.time/1000 + m_q.data[i]; double diff = m_qRef.data[i] - m_qCurrent.data[i]; if ( diff > 0 ) diff = min(diff, 0.034907); // 2 [deg] diff --git a/rtc/GraspController/GraspController.h b/rtc/GraspController/GraspController.h index 565876c52b5..7e17b558cdc 100644 --- a/rtc/GraspController/GraspController.h +++ b/rtc/GraspController/GraspController.h @@ -10,13 +10,14 @@ #ifndef GRASP_CONTROLLER_H #define GRASP_CONTROLLER_H +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include #include #include #include -#include "HRPDataTypes.hh" // Service implementation headers // diff --git a/rtc/GraspController/GraspControllerService_impl.h b/rtc/GraspController/GraspControllerService_impl.h index 192baf45301..33d44c5accb 100644 --- a/rtc/GraspController/GraspControllerService_impl.h +++ b/rtc/GraspController/GraspControllerService_impl.h @@ -2,7 +2,7 @@ #ifndef __GRAP_CONTROLLER_SERVICE_H__ #define __GRAP_CONTROLLER_SERVICE_H__ -#include "GraspControllerService.hh" +#include "hrpsys/idl/GraspControllerService.hh" class GraspController; diff --git a/rtc/HGcontroller/CMakeLists.txt b/rtc/HGcontroller/CMakeLists.txt index 0d171c48355..485f13668ba 100644 --- a/rtc/HGcontroller/CMakeLists.txt +++ b/rtc/HGcontroller/CMakeLists.txt @@ -9,6 +9,6 @@ target_link_libraries(HGcontrollerComp ${OPENRTM_LIBRARIES}) set(target HGcontroller HGcontrollerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/HGcontroller/HGcontroller.h b/rtc/HGcontroller/HGcontroller.h index c99cbe37593..fd49154b348 100644 --- a/rtc/HGcontroller/HGcontroller.h +++ b/rtc/HGcontroller/HGcontroller.h @@ -10,6 +10,7 @@ #ifndef HG_CONTROLLER_H #define HG_CONTROLLER_H +#include #include #include #include diff --git a/rtc/ImageData2CameraImage/CMakeLists.txt b/rtc/ImageData2CameraImage/CMakeLists.txt index 48837a15b8d..133188ecabc 100644 --- a/rtc/ImageData2CameraImage/CMakeLists.txt +++ b/rtc/ImageData2CameraImage/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(ImageData2CameraImageComp ${libs}) set(target ImageData2CameraImage ImageData2CameraImageComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ImageData2CameraImage/ImageData2CameraImage.h b/rtc/ImageData2CameraImage/ImageData2CameraImage.h index ff17d7b40c7..603fa057a68 100644 --- a/rtc/ImageData2CameraImage/ImageData2CameraImage.h +++ b/rtc/ImageData2CameraImage/ImageData2CameraImage.h @@ -10,13 +10,14 @@ #ifndef IMAGEDATA_2_CAMERAIMAGE_H #define IMAGEDATA_2_CAMERAIMAGE_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/ImpedanceController/CMakeLists.txt b/rtc/ImpedanceController/CMakeLists.txt index 27f5a14e055..d73c54a8335 100644 --- a/rtc/ImpedanceController/CMakeLists.txt +++ b/rtc/ImpedanceController/CMakeLists.txt @@ -1,4 +1,4 @@ -set(comp_sources ImpedanceController.cpp ImpedanceControllerService_impl.cpp JointPathEx.cpp RatsMatrix.cpp ImpedanceOutputGenerator.h ObjectTurnaroundDetector.h ../TorqueFilter/IIRFilter.cpp) +set(comp_sources ImpedanceController.cpp ImpedanceControllerService_impl.cpp JointPathEx.cpp RatsMatrix.cpp ImpedanceOutputGenerator.h ../TorqueFilter/IIRFilter.cpp) set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) add_library(ImpedanceController SHARED ${comp_sources}) target_link_libraries(ImpedanceController ${libs}) @@ -9,19 +9,17 @@ target_link_libraries(ImpedanceControllerComp ${libs}) add_executable(testImpedanceOutputGenerator testImpedanceOutputGenerator.cpp ImpedanceOutputGenerator.h RatsMatrix.cpp) target_link_libraries(testImpedanceOutputGenerator ${libs}) -add_executable(testObjectTurnaroundDetector testObjectTurnaroundDetector.cpp ObjectTurnaroundDetector.h ../TorqueFilter/IIRFilter.cpp) -target_link_libraries(testObjectTurnaroundDetector ${libs}) add_library(JointPathExC SHARED JointPathExC.cpp JointPathEx.cpp) target_link_libraries(JointPathExC ${libs}) -set(target ImpedanceController ImpedanceControllerComp testImpedanceOutputGenerator testObjectTurnaroundDetector JointPathExC) +set(target ImpedanceController ImpedanceControllerComp testImpedanceOutputGenerator JointPathExC) add_test(testImpedanceOutputGeneratorTest0 testImpedanceOutputGenerator --test0 --use-gnuplot false) add_test(testImpedanceOutputGeneratorTest1 testImpedanceOutputGenerator --test1 --use-gnuplot false) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index 8f428a71b72..49c750a6027 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -15,7 +15,7 @@ #include "JointPathEx.h" #include #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" #include #define MAX_TRANSITION_COUNT (static_cast(2/m_dt)) @@ -120,20 +120,20 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() // Setting for wrench data ports (real + virtual) std::vector fsensor_names; // find names for real force sensors - int npforce = m_robot->numSensors(hrp::Sensor::FORCE); + unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE); for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); } // load virtual force sensors readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name)); - int nvforce = m_vfs.size(); + unsigned int nvforce = m_vfs.size(); for (unsigned int i=0; i::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) { - if (it->second.id == i) fsensor_names.push_back(it->first); + if (it->second.id == (int)i) fsensor_names.push_back(it->first); } } // add ports for all force sensors - int nforce = npforce + nvforce; + unsigned int nforce = npforce + nvforce; m_force.resize(nforce); m_forceIn.resize(nforce); m_ref_force.resize(nforce); @@ -158,8 +158,8 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() } unsigned int dof = m_robot->numJoints(); - for ( int i = 0 ; i < dof; i++ ){ - if ( i != m_robot->joint(i)->jointId ) { + for ( unsigned int i = 0 ; i < dof; i++ ){ + if ( (int)i != m_robot->joint(i)->jointId ) { std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl; return RTC::RTC_ERROR; } @@ -201,6 +201,17 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() for (unsigned int i=0; iname(); hrp::ForceSensor* sensor = m_robot->sensor(sensor_name); + std::string sensor_link_name; + if ( sensor ) { + // real force sensor + sensor_link_name = sensor->link->name; + } else if ( m_vfs.find(sensor_name) != m_vfs.end()) { + // virtual force sensor + sensor_link_name = m_vfs[sensor_name].link->name; + } else { + std::cerr << "[" << m_profile.instance_name << "] unknown force param" << std::endl; + continue; + } // 1. Check whether adequate ee_map exists for the sensor. std::string ee_name; bool is_ee_exists = false; @@ -208,7 +219,7 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() hrp::Link* alink = m_robot->link(it->second.target_name); std::string tmp_base_name = base_name_map[it->first]; while (alink != NULL && alink->name != tmp_base_name && !is_ee_exists) { - if ( alink->name == sensor->link->name ) { + if ( alink->name == sensor_link_name ) { is_ee_exists = true; ee_name = it->first; } @@ -216,12 +227,12 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() } } if (!is_ee_exists) { - std::cerr << "[" << m_profile.instance_name << "] No such ee setting for " << sensor_name << " and " << sensor->link->name << "!!. Impedance param for " << sensor_name << " cannot be added!!" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] No such ee setting for " << sensor_name << " and " << sensor_link_name << "!!. Impedance param for " << sensor_name << " cannot be added!!" << std::endl; continue; } // 2. Check whether already impedance param exists, which has the same target link as the sensor. if (m_impedance_param.find(ee_name) != m_impedance_param.end()) { - std::cerr << "[" << m_profile.instance_name << "] Already impedance param (target_name=" << sensor->link->name << ", ee_name=" << ee_name << ") exists!!. Impedance param for " << sensor_name << " cannot be added!!" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Already impedance param (target_name=" << sensor_link_name << ", ee_name=" << ee_name << ") exists!!. Impedance param for " << sensor_name << " cannot be added!!" << std::endl; continue; } // 3. Check whether joint path is adequate. @@ -236,7 +247,7 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() p.transition_joint_q.resize(m_robot->numJoints()); p.sensor_name = sensor_name; m_impedance_param[ee_name] = p; - std::cerr << "[" << m_profile.instance_name << "] sensor = " << sensor_name << ", sensor-link = " << sensor->link->name << ", ee_name = " << ee_name << ", ee-link = " << target_link->name << std::endl; + std::cerr << "[" << m_profile.instance_name << "] sensor = " << sensor_name << ", sensor-link = " << sensor_link_name << ", ee_name = " << ee_name << ", ee-link = " << target_link->name << std::endl; } std::vector > interlocking_joints; @@ -248,9 +259,6 @@ RTC::ReturnCode_t ImpedanceController::onInitialize() } } - otd = boost::shared_ptr(new ObjectTurnaroundDetector(m_dt)); - otd->setPrintStr(std::string(m_profile.instance_name)); - // allocate memory for outPorts m_q.data.length(dof); qrefv.resize(dof); @@ -335,7 +343,7 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) if ( DEBUGP ) { std::cerr << "[" << m_profile.instance_name << "] qRef = "; - for ( int i = 0; i < m_qRef.data.length(); i++ ){ + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){ std::cerr << " " << m_qRef.data[i]; } std::cerr << std::endl; @@ -343,86 +351,21 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) Guard guard(m_mutex); - bool is_active = false; - for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { - is_active = is_active || it->second.is_active; - } - if ( !is_active ) { - for ( int i = 0; i < m_qRef.data.length(); i++ ){ - m_q.data[i] = m_qRef.data[i]; - m_robot->joint(i)->q = m_qRef.data[i]; - } - m_qOut.write(); - return RTC_OK; - } - { + // Store current robot state hrp::dvector qorg(m_robot->numJoints()); - - // reference model - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ qorg[i] = m_robot->joint(i)->q; - m_robot->joint(i)->q = m_qRef.data[i]; - qrefv[i] = m_qRef.data[i]; - } - m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z); - m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y); - m_robot->calcForwardKinematics(); - if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot - && !use_sh_base_pos_rpy ) { - // TODO - // Tempolarily modify root coords to fix foot pos rot - // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 - - // get current foot mid pos + rot - std::vector foot_pos; - std::vector foot_rot; - std::vector leg_names; - leg_names.push_back("rleg"); - leg_names.push_back("lleg"); - for (size_t i = 0; i < leg_names.size(); i++) { - hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); - foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos); - foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); - } - hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); - hrp::Matrix33 current_foot_mid_rot; - rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); - // calculate fix pos + rot - hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos); - hrp::Matrix33 new_foot_mid_rot; - { - hrp::Vector3 ex = hrp::Vector3::UnitX(); - hrp::Vector3 ez = hrp::Vector3::UnitZ(); - hrp::Vector3 xv1 (current_foot_mid_rot * ex); - xv1(2) = 0.0; - xv1.normalize(); - hrp::Vector3 yv1(ez.cross(xv1)); - new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2); - new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2); - new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2); - } - // fix root pos + rot to fix "coords" = "current_foot_mid_xx" - hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose()); - m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); - rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); - m_robot->calcForwardKinematics(); } + // Get target parameters mainly from SequencePlayer + getTargetParameters(); + // Calculate act/ref absolute force/moment calcForceMoment(); - - // set sequencer position to target_p0 - for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { - ImpedanceParam& param = it->second; - std::string target_name = ee_map[it->first].target_name; - param.target_p0 = m_robot->link(target_name)->p + m_robot->link(target_name)->R * ee_map[it->first].localPos; - param.target_r0 = m_robot->link(target_name)->R * ee_map[it->first].localR; - if (param.transition_count == -MAX_TRANSITION_COUNT) param.resetPreviousTargetParam(); - } // back to impedance robot model (only for controlled joint) for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { ImpedanceParam& param = it->second; if (param.is_active) { - for ( int j = 0; j < param.manip->numJoints(); j++ ){ + for ( unsigned int j = 0; j < param.manip->numJoints(); j++ ){ int i = param.manip->joint(j)->jointId; m_robot->joint(i)->q = qorg[i]; } @@ -432,86 +375,37 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) } - // set m_robot to qRef when deleting status - std::map::iterator it = m_impedance_param.begin(); - while(it != m_impedance_param.end()){ - ImpedanceParam& param = it->second; - if (param.is_active) { - if (DEBUGP) { - std::cerr << "[" << m_profile.instance_name << "] impedance mode " << it->first << " transition count = " << param.transition_count << ", "; - std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << ", "; - std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << ", "; - std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << ", "; - std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl; - } - if ( param.transition_count > 0 ) { - hrp::JointPathExPtr manip = param.manip; - for ( int j = 0; j < manip->numJoints(); j++ ) { - int i = manip->joint(j)->jointId; // index in robot model - hrp::Link* joint = m_robot->joint(i); - // transition_smooth_gain moves from 0 to 1 - // (/ (log (/ (- 1 0.99) 0.99)) 0.5) - double transition_smooth_gain = 1/(1+exp(-9.19*((static_cast(MAX_TRANSITION_COUNT - param.transition_count) / MAX_TRANSITION_COUNT) - 0.5))); - joint->q = ( m_qRef.data[i] - param.transition_joint_q[i] ) * transition_smooth_gain + param.transition_joint_q[i]; - } - param.transition_count--; - if(param.transition_count <= 0){ // erase impedance param - std::cerr << "[" << m_profile.instance_name << "] Finished cleanup and erase impedance param " << it->first << std::endl; - param.is_active = false; - } - } else { - // use impedance model - - hrp::Link* target = m_robot->link(ee_map[it->first].target_name); - assert(target); - param.current_p1 = target->p + target->R * ee_map[it->first].localPos; - param.current_r1 = target->R * ee_map[it->first].localR; - if (param.transition_count == -MAX_TRANSITION_COUNT) param.resetPreviousCurrentParam(); - - hrp::Vector3 vel_p, vel_r; - //std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << std::endl; - //std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << std::endl; - // std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << std::endl; - // std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl; - - // ref_force/ref_moment and force_gain/moment_gain are expressed in global coordinates. - hrp::Matrix33 eeR = target->R * ee_map[it->first].localR; - hrp::Vector3 force_diff = abs_forces[it->second.sensor_name] - abs_ref_forces[it->second.sensor_name]; - hrp::Vector3 moment_diff = abs_moments[it->second.sensor_name] - abs_ref_moments[it->second.sensor_name]; - param.calcTargetVelocity(vel_p, vel_r, eeR, force_diff, moment_diff, m_dt, - DEBUGP, std::string(m_profile.instance_name), it->first); - - // Solve ik - hrp::JointPathExPtr manip = param.manip; - assert(manip); - //const int n = manip->numJoints(); - manip->calcInverseKinematics2Loop(param.getOutputPos(), param.getOutputRot(), 1.0, param.avoid_gain, param.reference_gain, &qrefv, 1.0, - ee_map[it->first].localPos, ee_map[it->first].localR); - - if ( param.transition_count < 0 ) { - param.transition_count++; - } - } // else - } - it++; - } // while + // Check if all limb is non-is_active mode. + bool is_active = false; + for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { + is_active = is_active || it->second.is_active; + } + if ( !is_active ) { + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){ + m_q.data[i] = m_qRef.data[i]; + m_robot->joint(i)->q = m_qRef.data[i]; + } + m_qOut.write(); + return RTC_OK; + } + + // Caculate ImpedanceControl and solve IK + calcImpedanceControl(); + // Output if ( m_q.data.length() != 0 ) { // initialized - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_q.data[i] = m_robot->joint(i)->q; } m_qOut.write(); if ( DEBUGP ) { std::cerr << "[" << m_profile.instance_name << "] q = "; - for ( int i = 0; i < m_q.data.length(); i++ ){ + for ( unsigned int i = 0; i < m_q.data.length(); i++ ){ std::cerr << " " << m_q.data[i]; } std::cerr << std::endl; } } - - if (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot - calcObjectTurnaroundDetectorState(); } else { if ( DEBUGP || loop % 100 == 0 ) { std::cerr << "ImpedanceController is not working..." << std::endl; @@ -557,19 +451,67 @@ RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) } */ -void ImpedanceController::calcFootMidCoords (hrp::Vector3& new_foot_mid_pos, hrp::Matrix33& new_foot_mid_rot) +void ImpedanceController::getTargetParameters () { - std::vector foot_pos; - std::vector foot_rot; - std::vector leg_names = boost::assign::list_of("rleg")("lleg"); - for (size_t i = 0; i < leg_names.size(); i++) { - hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); - foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos); - foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); - } - new_foot_mid_pos = (foot_pos[0]+foot_pos[1])/2.0; - rats::mid_rot(new_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); -} + // Get reference robot state + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = m_qRef.data[i]; + qrefv[i] = m_qRef.data[i]; + } + m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z); + m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y); + m_robot->calcForwardKinematics(); + // Fix leg for legged robot + if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot + && !use_sh_base_pos_rpy ) { + // TODO + // Tempolarily modify root coords to fix foot pos rot + // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 + + // get current foot mid pos + rot + std::vector foot_pos; + std::vector foot_rot; + std::vector leg_names; + leg_names.push_back("rleg"); + leg_names.push_back("lleg"); + for (size_t i = 0; i < leg_names.size(); i++) { + hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); + foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos); + foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); + } + hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); + hrp::Matrix33 current_foot_mid_rot; + rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + // calculate fix pos + rot + hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos); + hrp::Matrix33 new_foot_mid_rot; + { + hrp::Vector3 ex = hrp::Vector3::UnitX(); + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 xv1 (current_foot_mid_rot * ex); + xv1(2) = 0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2); + new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2); + new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2); + } + // fix root pos + rot to fix "coords" = "current_foot_mid_xx" + hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose()); + m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); + rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); + m_robot->calcForwardKinematics(); + } + + // Set sequencer position and orientation to target_p0 and target_r0 + for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { + ImpedanceParam& param = it->second; + std::string target_name = ee_map[it->first].target_name; + param.target_p0 = m_robot->link(target_name)->p + m_robot->link(target_name)->R * ee_map[it->first].localPos; + param.target_r0 = m_robot->link(target_name)->R * ee_map[it->first].localR; + if (param.transition_count == -MAX_TRANSITION_COUNT) param.resetPreviousTargetParam(); + } +}; void ImpedanceController::calcForceMoment () { @@ -591,7 +533,7 @@ void ImpedanceController::calcForceMoment () hrp::Matrix33 sensorR; hrp::Vector3 sensorPos, eePos; if ( sensor ) { - // real force sensore + // real force sensor sensorR = sensor->link->R * sensor->localR; sensorPos = sensor->link->p + sensorR * sensor->localPos; } else if ( m_vfs.find(sensor_name) != m_vfs.end()) { @@ -606,11 +548,7 @@ void ImpedanceController::calcForceMoment () } abs_forces[sensor_name] = sensorR * data_p; for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { - if ( it->second.sensor_name == sensor_name ) { - std::string ee_name = it->first; - hrp::Link* target_link = m_robot->link(ee_map[ee_name].target_name); - eePos = target_link->p + target_link->R * ee_map[ee_name].localPos; - } + if ( it->second.sensor_name == sensor_name ) eePos = it->second.target_p0; } abs_moments[sensor_name] = sensorR * data_r + (sensorPos - eePos).cross(abs_forces[sensor_name]); // End effector local frame @@ -630,52 +568,72 @@ void ImpedanceController::calcForceMoment () } }; -void ImpedanceController::calcObjectTurnaroundDetectorState() +void ImpedanceController::calcImpedanceControl () { - // TODO - // Currently only for legged robots - // Store org state - hrp::dvector org_q(m_robot->numJoints()); - for ( int i = 0; i < m_robot->numJoints(); i++ ) { - org_q[i] = m_robot->joint(i)->q; - } - hrp::Matrix33 orgR = m_robot->rootLink()->R; - // Set actual state - for ( int i = 0; i < m_robot->numJoints(); i++ ) { - m_robot->joint(i)->q = m_qCurrent.data[i]; - } - updateRootLinkPosRot(m_rpy); - // Calc - std::vector otd_fmv, otd_hposv; - hrp::Vector3 fmpos; - hrp::Matrix33 fmrot, fmrotT; - calcFootMidCoords(fmpos, fmrot); - fmrotT = fmrot.transpose(); - for (unsigned int i=0; iname(); - if ( find(otd_sensor_names.begin(), otd_sensor_names.end(), sensor_name) != otd_sensor_names.end() ) { - hrp::ForceSensor* sensor = m_robot->sensor(sensor_name); - hrp::Vector3 data_p(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]); - hrp::Vector3 data_r(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5]); - hrp::Matrix33 sensorR = sensor->link->R * sensor->localR; - otd_fmv.push_back(fmrotT*(sensorR*data_p)); - hrp::Vector3 eePos; - for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { - if ( it->second.sensor_name == sensor_name ) { - ee_trans& eet = ee_map[it->first]; - hrp::Link* target_link = m_robot->link(eet.target_name); - eePos = fmrotT * (target_link->p + target_link->R * eet.localPos - fmpos); - } + std::map::iterator it = m_impedance_param.begin(); + while(it != m_impedance_param.end()){ + ImpedanceParam& param = it->second; + if (param.is_active) { + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] impedance mode " << it->first << " transition count = " << param.transition_count << ", "; + std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << ", "; + std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << ", "; + std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << ", "; + std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl; } - otd_hposv.push_back(eePos); + if ( param.transition_count > 0 ) { + // set m_robot to qRef when deleting status + hrp::JointPathExPtr manip = param.manip; + // transition_smooth_gain moves from 0 to 1 + // (/ (log (/ (- 1 0.99) 0.99)) 0.5) + double transition_smooth_gain = 1/(1+exp(-9.19*((static_cast(MAX_TRANSITION_COUNT - param.transition_count) / MAX_TRANSITION_COUNT) - 0.5))); + for ( unsigned int j = 0; j < manip->numJoints(); j++ ) { + int i = manip->joint(j)->jointId; // index in robot model + hrp::Link* joint = m_robot->joint(i); + joint->q = ( m_qRef.data[i] - param.transition_joint_q[i] ) * transition_smooth_gain + param.transition_joint_q[i]; + } + // transition_count update + param.transition_count--; + if(param.transition_count <= 0){ // erase impedance param + std::cerr << "[" << m_profile.instance_name << "] Finished cleanup and erase impedance param " << it->first << std::endl; + param.is_active = false; + } + } else { + // use impedance model + + hrp::Link* target = m_robot->link(ee_map[it->first].target_name); + assert(target); + param.current_p1 = target->p + target->R * ee_map[it->first].localPos; + param.current_r1 = target->R * ee_map[it->first].localR; + if (param.transition_count == -MAX_TRANSITION_COUNT) param.resetPreviousCurrentParam(); + + hrp::Vector3 vel_p, vel_r; + //std::cerr << "MDK = " << param.M_p << " " << param.D_p << " " << param.K_p << std::endl; + //std::cerr << "MDK = " << param.M_r << " " << param.D_r << " " << param.K_r << std::endl; + // std::cerr << "ref_force = " << param.ref_force[0] << " " << param.ref_force[1] << " " << param.ref_force[2] << std::endl; + // std::cerr << "ref_moment = " << param.ref_moment[0] << " " << param.ref_moment[1] << " " << param.ref_moment[2] << std::endl; + + // ref_force/ref_moment and force_gain/moment_gain are expressed in global coordinates. + hrp::Matrix33 eeR = target->R * ee_map[it->first].localR; + hrp::Vector3 force_diff = abs_forces[it->second.sensor_name] - abs_ref_forces[it->second.sensor_name]; + hrp::Vector3 moment_diff = abs_moments[it->second.sensor_name] - abs_ref_moments[it->second.sensor_name]; + param.calcTargetVelocity(vel_p, vel_r, eeR, force_diff, moment_diff, m_dt, + DEBUGP, std::string(m_profile.instance_name), it->first); + + // Solve ik + hrp::JointPathExPtr manip = param.manip; + assert(manip); + //const int n = manip->numJoints(); + manip->calcInverseKinematics2Loop(param.getOutputPos(), param.getOutputRot(), 1.0, param.avoid_gain, param.reference_gain, &qrefv, 1.0, + ee_map[it->first].localPos, ee_map[it->first].localR); + // transition_count update + if ( param.transition_count < 0 ) { + param.transition_count++; + } + } // else } - } - otd->checkDetection(otd_fmv, otd_hposv); - // Revert to org state - for ( int i = 0; i < m_robot->numJoints(); i++ ) { - m_robot->joint(i)->q = org_q[i]; - } - m_robot->rootLink()->R = orgR; + it++; + } // while }; // @@ -720,7 +678,7 @@ bool ImpedanceController::stopImpedanceControllerNoWait(const std::string& i_nam return false; } std::cerr << "[" << m_profile.instance_name << "] Stop impedance control [" << i_name_ << "]" << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++ ) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++ ) { m_impedance_param[i_name_].transition_joint_q[i] = m_robot->joint(i)->q; } m_impedance_param[i_name_].transition_count = MAX_TRANSITION_COUNT; // when stop impedance, count down to 0 @@ -849,103 +807,6 @@ void ImpedanceController::waitImpedanceControllerTransition(std::string i_name_) return; } -// -// ObjectTurnaroundDetector -// - -void ImpedanceController::startObjectTurnaroundDetection(const double i_ref_diff_wrench, const double i_max_time, const OpenHRP::ImpedanceControllerService::StrSequence& i_ee_names) -{ - otd->startDetection(i_ref_diff_wrench, i_max_time); - otd_sensor_names.clear(); - for (size_t i = 0; i < i_ee_names.length(); i++) { - otd_sensor_names.push_back(m_impedance_param[std::string(i_ee_names[i])].sensor_name); - } -} - -OpenHRP::ImpedanceControllerService::DetectorMode ImpedanceController::checkObjectTurnaroundDetection() -{ - OpenHRP::ImpedanceControllerService::DetectorMode tmpmode; - switch (otd->getMode()) { - case ObjectTurnaroundDetector::MODE_IDLE: - tmpmode = ImpedanceControllerService::MODE_DETECTOR_IDLE; - break; - case ObjectTurnaroundDetector::MODE_STARTED: - tmpmode = ImpedanceControllerService::MODE_STARTED; - break; - case ObjectTurnaroundDetector::MODE_DETECTED: - tmpmode = ImpedanceControllerService::MODE_DETECTED; - break; - case ObjectTurnaroundDetector::MODE_MAX_TIME: - tmpmode = ImpedanceControllerService::MODE_MAX_TIME; - break; - default: - tmpmode = ImpedanceControllerService::MODE_DETECTOR_IDLE; - break; - } - return tmpmode; -} - -bool ImpedanceController::setObjectTurnaroundDetectorParam(const OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam &i_param_) -{ - std::cerr << "[" << m_profile.instance_name << "] setObjectTurnaroundDetectorParam" << std::endl; - Guard guard(m_mutex); - otd->setWrenchCutoffFreq(i_param_.wrench_cutoff_freq); - otd->setDwrenchCutoffFreq(i_param_.dwrench_cutoff_freq); - otd->setDetectRatioThre(i_param_.detect_ratio_thre); - otd->setStartRatioThre(i_param_.start_ratio_thre); - otd->setDetectTimeThre(i_param_.detect_time_thre); - otd->setStartTimeThre(i_param_.start_time_thre); - hrp::Vector3 tmp; - for (size_t i = 0; i < 3; i++) tmp(i) = i_param_.axis[i]; - otd->setAxis(tmp); - for (size_t i = 0; i < 3; i++) tmp(i) = i_param_.moment_center[i]; - otd->setMomentCenter(tmp); - otd->setDetectorTotalWrench((i_param_.detector_total_wrench==OpenHRP::ImpedanceControllerService::TOTAL_FORCE)?ObjectTurnaroundDetector::TOTAL_FORCE:ObjectTurnaroundDetector::TOTAL_MOMENT); - otd->printParams(); - return true; -}; - -bool ImpedanceController::getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam& i_param_) -{ - std::cerr << "[" << m_profile.instance_name << "] getObjectTurnaroundDetectorParam" << std::endl; - i_param_.wrench_cutoff_freq = otd->getWrenchCutoffFreq(); - i_param_.dwrench_cutoff_freq = otd->getDwrenchCutoffFreq(); - i_param_.detect_ratio_thre = otd->getDetectRatioThre(); - i_param_.start_ratio_thre = otd->getStartRatioThre(); - i_param_.detect_time_thre = otd->getDetectTimeThre(); - i_param_.start_time_thre = otd->getStartTimeThre(); - hrp::Vector3 tmp = otd->getAxis(); - for (size_t i = 0; i < 3; i++) i_param_.axis[i] = tmp(i); - tmp = otd->getMomentCenter(); - for (size_t i = 0; i < 3; i++) i_param_.moment_center[i] = tmp(i); - i_param_.detector_total_wrench = (otd->getDetectorTotalWrench()==ObjectTurnaroundDetector::TOTAL_FORCE)?OpenHRP::ImpedanceControllerService::TOTAL_FORCE:OpenHRP::ImpedanceControllerService::TOTAL_MOMENT; - return true; -} - -bool ImpedanceController::getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench) -{ - std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl; - if (otd_sensor_names.size() == 0) return false; - hrp::Vector3 tmpv = otd->getAxis() * otd->getFilteredWrench(); - o_forces = new OpenHRP::ImpedanceControllerService::Dbl3Sequence (); - o_moments = new OpenHRP::ImpedanceControllerService::Dbl3Sequence (); - o_forces->length(otd_sensor_names.size()); - o_moments->length(otd_sensor_names.size()); - for (size_t i = 0; i < o_forces->length(); i++) o_forces[i].length(3); - for (size_t i = 0; i < o_moments->length(); i++) o_moments[i].length(3); - // Temp - for (size_t i = 0; i < otd_sensor_names.size(); i++) { - o_forces[i][0] = tmpv(0)/otd_sensor_names.size(); - o_forces[i][1] = tmpv(1)/otd_sensor_names.size(); - o_forces[i][2] = tmpv(2)/otd_sensor_names.size(); - o_moments[i][0] = o_moments[i][1] = o_moments[i][2] = 0.0; - } - o_3dofwrench = new OpenHRP::ImpedanceControllerService::DblSequence3 (); - o_3dofwrench->length(3); - for (size_t i = 0; i < 3; i++) (*o_3dofwrench)[i] = tmpv(i); - return true; -} - extern "C" { @@ -958,5 +819,3 @@ extern "C" } }; - - diff --git a/rtc/ImpedanceController/ImpedanceController.h b/rtc/ImpedanceController/ImpedanceController.h index 4eacd86ddaf..e4bc3e4fba4 100644 --- a/rtc/ImpedanceController/ImpedanceController.h +++ b/rtc/ImpedanceController/ImpedanceController.h @@ -10,6 +10,8 @@ #ifndef IMPEDANCE_H #define IMPEDANCE_H +#include +#include #include #include #include @@ -21,7 +23,6 @@ #include "JointPathEx.h" #include "RatsMatrix.h" #include "ImpedanceOutputGenerator.h" -#include "ObjectTurnaroundDetector.h" // Service implementation headers // #include "ImpedanceControllerService_impl.h" @@ -97,11 +98,6 @@ class ImpedanceController bool setImpedanceControllerParam(const std::string& i_name_, OpenHRP::ImpedanceControllerService::impedanceParam i_param_); bool getImpedanceControllerParam(const std::string& i_name_, OpenHRP::ImpedanceControllerService::impedanceParam& i_param_); void waitImpedanceControllerTransition(std::string i_name_); - void startObjectTurnaroundDetection(const double i_ref_diff_wrench, const double i_max_time, const OpenHRP::ImpedanceControllerService::StrSequence& i_ee_names); - OpenHRP::ImpedanceControllerService::DetectorMode checkObjectTurnaroundDetection(); - bool setObjectTurnaroundDetectorParam(const OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam &i_param_); - bool getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam& i_param_); - bool getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench); protected: // Configuration variable declaration @@ -177,17 +173,14 @@ class ImpedanceController void copyImpedanceParam (OpenHRP::ImpedanceControllerService::impedanceParam& i_param_, const ImpedanceParam& param); void updateRootLinkPosRot (TimedOrientation3D tmprpy); - void calcFootMidCoords (hrp::Vector3& new_foot_mid_pos, hrp::Matrix33& new_foot_mid_rot); + void getTargetParameters (); + void calcImpedanceControl (); void calcForceMoment(); - void calcObjectTurnaroundDetectorState(); std::map m_impedance_param; std::map ee_map; std::map m_vfs; std::map abs_forces, abs_moments, abs_ref_forces, abs_ref_moments; - boost::shared_ptr otd; - std::vector otd_sensor_names; - hrp::Vector3 otd_axis; double m_dt; hrp::BodyPtr m_robot; coil::Mutex m_mutex; diff --git a/rtc/ImpedanceController/ImpedanceControllerService_impl.cpp b/rtc/ImpedanceController/ImpedanceControllerService_impl.cpp index 18c3588a491..92ecdbe5e6a 100644 --- a/rtc/ImpedanceController/ImpedanceControllerService_impl.cpp +++ b/rtc/ImpedanceController/ImpedanceControllerService_impl.cpp @@ -49,32 +49,6 @@ void ImpedanceControllerService_impl::waitImpedanceControllerTransition(const ch m_impedance->waitImpedanceControllerTransition(std::string(i_name_)); } -void ImpedanceControllerService_impl::startObjectTurnaroundDetection(const CORBA::Double i_ref_diff_wrench, const CORBA::Double i_max_time, const OpenHRP::ImpedanceControllerService::StrSequence& i_ee_names) -{ - m_impedance->startObjectTurnaroundDetection(i_ref_diff_wrench, i_max_time, i_ee_names); -} - -OpenHRP::ImpedanceControllerService::DetectorMode ImpedanceControllerService_impl::checkObjectTurnaroundDetection() -{ - return m_impedance->checkObjectTurnaroundDetection(); -} - -CORBA::Boolean ImpedanceControllerService_impl::setObjectTurnaroundDetectorParam(const OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam &i_param_) -{ - return m_impedance->setObjectTurnaroundDetectorParam(i_param_); -} - -CORBA::Boolean ImpedanceControllerService_impl::getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam& i_param_) -{ - i_param_ = OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam(); - return m_impedance->getObjectTurnaroundDetectorParam(i_param_); -} - -CORBA::Boolean ImpedanceControllerService_impl::getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench) -{ - return m_impedance->getObjectForcesMoments(o_forces, o_moments, o_3dofwrench); -} - void ImpedanceControllerService_impl::impedance(ImpedanceController *i_impedance) { m_impedance = i_impedance; diff --git a/rtc/ImpedanceController/ImpedanceControllerService_impl.h b/rtc/ImpedanceController/ImpedanceControllerService_impl.h index 0f2f2291ee8..5856261adbe 100644 --- a/rtc/ImpedanceController/ImpedanceControllerService_impl.h +++ b/rtc/ImpedanceController/ImpedanceControllerService_impl.h @@ -2,7 +2,7 @@ #ifndef IMPEDANCESERVICESVC_IMPL_H #define IMPEDANCESERVICESVC_IMPL_H -#include "ImpedanceControllerService.hh" +#include "hrpsys/idl/ImpedanceControllerService.hh" using namespace OpenHRP; @@ -23,11 +23,6 @@ class ImpedanceControllerService_impl CORBA::Boolean setImpedanceControllerParam(const char *i_name_, const OpenHRP::ImpedanceControllerService::impedanceParam &i_param_); CORBA::Boolean getImpedanceControllerParam(const char *i_name_, OpenHRP::ImpedanceControllerService::impedanceParam_out i_param_); void waitImpedanceControllerTransition(const char *i_name_); - void startObjectTurnaroundDetection(const CORBA::Double i_ref_diff_wrench, const CORBA::Double i_max_time, const OpenHRP::ImpedanceControllerService::StrSequence& i_ee_names); - OpenHRP::ImpedanceControllerService::DetectorMode checkObjectTurnaroundDetection(); - CORBA::Boolean setObjectTurnaroundDetectorParam(const OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam &i_param_); - CORBA::Boolean getObjectTurnaroundDetectorParam(OpenHRP::ImpedanceControllerService::objectTurnaroundDetectorParam& i_param_); - CORBA::Boolean getObjectForcesMoments(OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_forces, OpenHRP::ImpedanceControllerService::Dbl3Sequence_out o_moments, OpenHRP::ImpedanceControllerService::DblSequence3_out o_3dofwrench); // void impedance(ImpedanceController *i_impedance); diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp index c0dfa3d2f5f..2d7fba1ce7f 100644 --- a/rtc/ImpedanceController/JointPathEx.cpp +++ b/rtc/ImpedanceController/JointPathEx.cpp @@ -20,6 +20,7 @@ std::ostream& operator<<(std::ostream& out, hrp::dmatrix &a) { } out << std::endl; } + return out; } std::ostream& operator<<(std::ostream& out, hrp::dvector &a) { @@ -29,6 +30,7 @@ std::ostream& operator<<(std::ostream& out, hrp::dvector &a) { out << std::setw(7) << std::setiosflags(std::ios::fixed) << std::setprecision(4) << a(i) << " "; } out << std::endl; + return out; } //#define DEBUG true @@ -59,6 +61,7 @@ int hrp::calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio, dmat _a_sr = _w * at * a1; //if (DEBUG) { dmatrix ii = _a * _a_sr; std::cerr << " i :" << std::endl << ii; } + return 0; } // overwrite hrplib/hrpUtil/Eigen3d.cpp @@ -87,16 +90,31 @@ Vector3 omegaFromRotEx(const Matrix33& r) } JointPathEx::JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval, const std::string& _debug_print_prefix) - : JointPath(base, end), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50), interlocking_joint_pair_indices(), dt(control_cycle), + : JointPath(base, end), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50), interlocking_joint_pair_indices(), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), dt(control_cycle), debug_print_prefix(_debug_print_prefix+",JointPathEx"), joint_limit_debug_print_counts(numJoints(), 0), debug_print_freq_count(static_cast(0.25/dt)), // once per 0.25[s] use_inside_joint_weight_retrieval(_use_inside_joint_weight_retrieval) { - for (int i = 0 ; i < numJoints(); i++ ) { + for (unsigned int i = 0 ; i < numJoints(); i++ ) { joints.push_back(joint(i)); } avoid_weight_gain.resize(numJoints()); optional_weight_vector.resize(numJoints()); - for (int i = 0 ; i < numJoints(); i++ ) { + for (unsigned int i = 0 ; i < numJoints(); i++ ) { + optional_weight_vector[i] = 1.0; + } +} + +JointPathEx::JointPathEx(BodyPtr& robot, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval, const std::string& _debug_print_prefix) + : JointPath( end), maxIKPosErrorSqr(1.0e-8), maxIKRotErrorSqr(1.0e-6), maxIKIteration(50), interlocking_joint_pair_indices(), sr_gain(1.0), manipulability_limit(0.1), manipulability_gain(0.001), dt(control_cycle), + debug_print_prefix(_debug_print_prefix+",JointPathEx"), joint_limit_debug_print_counts(numJoints(), 0), + debug_print_freq_count(static_cast(0.25/dt)), // once per 0.25[s] + use_inside_joint_weight_retrieval(_use_inside_joint_weight_retrieval) { + for (unsigned int i = 0 ; i < numJoints(); i++ ) { + joints.push_back(joint(i)); + } + avoid_weight_gain.resize(numJoints()); + optional_weight_vector.resize(numJoints()); + for (unsigned int i = 0 ; i < numJoints(); i++ ) { optional_weight_vector[i] = 1.0; } } @@ -188,7 +206,7 @@ bool JointPathEx::calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatri } else { r = fabs( (pow((jmax - jmin),2) * (( 2 * jang) - jmax - jmin)) / (4 * pow((jmax - jang),2) * pow((jang - jmin),2)) ); - if (isnan(r)) r = 0; + if (std::isnan(r)) r = 0; } // If use_inside_joint_weight_retrieval = true (true by default), use T. F. Chang and R.-V. Dubeby weight retrieval inward. @@ -316,7 +334,7 @@ bool JointPathEx::calcInverseKinematics2Loop(const Vector3& dp, const Vector3& o // // qref - qcurr hrp::dvector u(n); - for ( int j = 0; j < numJoints(); j++ ) { + for ( unsigned int j = 0; j < numJoints(); j++ ) { u[j] = optional_weight_vector[j] * reference_gain * ( (*reference_q)[joint(j)->jointId] - joint(j)->q ); } if ( DEBUG ) { @@ -370,7 +388,7 @@ bool JointPathEx::calcInverseKinematics2Loop(const Vector3& dp, const Vector3& o // check nan / inf bool solve_linear_equation = true; for(int j=0; j < n; ++j){ - if ( isnan(dq(j)) || isinf(dq(j)) ) { + if ( std::isnan(dq(j)) || std::isinf(dq(j)) ) { solve_linear_equation = false; break; } @@ -571,7 +589,7 @@ void hrp::readVirtualForceSensorParamFromProperties (std::mapnumJoints();i++)_idsb.q(i) = _m_robot->joint(i)->q; + _idsb.dq = (_idsb.q - _idsb.q_old) / _idsb.DT; + _idsb.ddq = (_idsb.q - 2 * _idsb.q_old + _idsb.q_oldold) / (_idsb.DT * _idsb.DT); + const hrp::Vector3 g(0, 0, 9.80665); + _idsb.base_p = _m_robot->rootLink()->p; + _idsb.base_v = (_idsb.base_p - _idsb.base_p_old) / _idsb.DT; + _idsb.base_dv = g + (_idsb.base_p - 2 * _idsb.base_p_old + _idsb.base_p_oldold) / (_idsb.DT * _idsb.DT); + _idsb.base_R = _m_robot->rootLink()->R; + _idsb.base_dR = (_idsb.base_R - _idsb.base_R_old) / _idsb.DT; + _idsb.base_w_hat = _idsb.base_dR * _idsb.base_R.transpose(); + _idsb.base_w = hrp::Vector3(_idsb.base_w_hat(2,1), - _idsb.base_w_hat(0,2), _idsb.base_w_hat(1,0)); + _idsb.base_dw = (_idsb.base_w - _idsb.base_w_old) / _idsb.DT; +}; + +void hrp::calcRootLinkWrenchFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _f_ans, hrp::Vector3& _t_ans){ + for(int i=0;i<_m_robot->numJoints();i++){ + _m_robot->joint(i)->dq = _idsb.dq(i); + _m_robot->joint(i)->ddq = _idsb.ddq(i); + } + _m_robot->rootLink()->vo = _idsb.base_v - _idsb.base_w.cross(_idsb.base_p); + _m_robot->rootLink()->dvo = _idsb.base_dv - _idsb.base_dw.cross(_idsb.base_p) - _idsb.base_w.cross(_idsb.base_v); // calc in differential way + _m_robot->rootLink()->w = _idsb.base_w; + _m_robot->rootLink()->dw = _idsb.base_dw; + _m_robot->calcForwardKinematics(true,true);// calc every link's acc and vel + _m_robot->calcInverseDynamics(_m_robot->rootLink(), _f_ans, _t_ans);// this returns f,t at the coordinate origin (not at base link pos) +}; + +void hrp::calcWorldZMPFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _zmp_ans){ + hrp::Vector3 f_tmp, t_tmp; + calcRootLinkWrenchFromInverseDynamics(_m_robot, _idsb, f_tmp, t_tmp); + _zmp_ans(0) = -t_tmp(1)/f_tmp(2); + _zmp_ans(1) = t_tmp(0)/f_tmp(2); +}; + +void hrp::updateInvDynStateBuffer(InvDynStateBuffer& _idsb){ + _idsb.q_oldold = _idsb.q_old; + _idsb.q_old = _idsb.q; + _idsb.base_p_oldold = _idsb.base_p_old; + _idsb.base_p_old = _idsb.base_p; + _idsb.base_R_old = _idsb.base_R; + _idsb.base_w_old = _idsb.base_w; +}; diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h index 2889cc508ab..3c6654eb517 100644 --- a/rtc/ImpedanceController/JointPathEx.h +++ b/rtc/ImpedanceController/JointPathEx.h @@ -16,6 +16,7 @@ namespace hrp { class JointPathEx : public JointPath { public: JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = ""); + JointPathEx(BodyPtr& robot, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = ""); bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull); bool calcInverseKinematics2Loop(const Vector3& dp, const Vector3& omega, const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL); bool calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R, @@ -24,16 +25,16 @@ namespace hrp { const hrp::Vector3& localPos = hrp::Vector3::Zero(), const hrp::Matrix33& localR = hrp::Matrix33::Identity()); bool calcInverseKinematics2(const Vector3& end_p, const Matrix33& end_R, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL); double getSRGain() { return sr_gain; } - bool setSRGain(double g) { sr_gain = g; } + bool setSRGain(double g) { sr_gain = g; return true; } double getManipulabilityLimit() { return manipulability_limit; } - bool setManipulabilityLimit(double l) { manipulability_limit = l; } - bool setManipulabilityGain(double l) { manipulability_gain = l; } + bool setManipulabilityLimit(double l) { manipulability_limit = l; return true; } + bool setManipulabilityGain(double l) { manipulability_gain = l; return true; } void setMaxIKError(double epos, double erot); void setMaxIKError(double e); void setMaxIKIteration(int iter); void setOptionalWeightVector(const std::vector& _opt_w) { - for (int i = 0 ; i < numJoints(); i++ ) { + for (unsigned int i = 0 ; i < numJoints(); i++ ) { optional_weight_vector[i] = _opt_w[i]; } }; @@ -42,7 +43,7 @@ namespace hrp { void getInterlockingJointPairIndices (std::vector >& pairs); void getOptionalWeightVector(std::vector& _opt_w) { - for (int i = 0 ; i < numJoints(); i++ ) { + for (unsigned int i = 0 ; i < numJoints(); i++ ) { _opt_w[i] = optional_weight_vector[i]; } }; @@ -84,6 +85,47 @@ namespace hrp { const std::string& instance_name); }; + +namespace hrp { + class InvDynStateBuffer{ + public: + int N_DOF; + bool is_initialized; + double DT; + hrp::dvector q, q_old, q_oldold, dq, ddq; + hrp::Vector3 base_p, base_p_old, base_p_oldold, base_v, base_dv; + hrp::Matrix33 base_R, base_R_old, base_dR, base_w_hat; + hrp::Vector3 base_w, base_w_old, base_dw; + InvDynStateBuffer():is_initialized(false){}; + ~InvDynStateBuffer(){}; + void setInitState(const hrp::BodyPtr _m_robot, const double _dt){ + N_DOF = _m_robot->numJoints(); + DT = _dt; + q.resize(N_DOF); + q_old.resize(N_DOF); + q_oldold.resize(N_DOF); + dq.resize(N_DOF); + ddq.resize(N_DOF); + for(int i=0;ijoint(i)->q; + q_oldold = q_old = q; + dq = ddq = hrp::dvector::Zero(N_DOF); + base_p_oldold = base_p_old = base_p = _m_robot->rootLink()->p; + base_R_old = base_R = _m_robot->rootLink()->R; + base_dR = base_w_hat = hrp::Matrix33::Zero(); + base_w_old = base_w = base_dw = hrp::Vector3::Zero(); + is_initialized = true; + }; + }; + // set current Body q,base_p,base_R into InvDynStateBuffer and update vel and acc + void calcAccelerationsForInverseDynamics(const hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb); + // set all vel and acc into Body, and call Body::calcInverseDynamics() + void calcRootLinkWrenchFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _f_ans, hrp::Vector3& _t_ans); + // call calcRootLinkWrenchFromInverseDynamics() and convert f,tau into ZMP + void calcWorldZMPFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _zmp_ans); + // increment InvDynStateBuffer for 1 step + void updateInvDynStateBuffer(InvDynStateBuffer& _idsb); +} + #include #endif //__JOINT_PATH_EX_H__ diff --git a/rtc/ImpedanceController/JointPathExC.cpp b/rtc/ImpedanceController/JointPathExC.cpp index ed748a93e37..c01abb3fd0b 100644 --- a/rtc/ImpedanceController/JointPathExC.cpp +++ b/rtc/ImpedanceController/JointPathExC.cpp @@ -65,7 +65,7 @@ extern "C" { int _setJointAngles (double* ja) { - for ( int i = 0; i < m_robot->numJoints(); i++ ) { + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) { m_robot->joint(i)->q = ja[i]; } m_robot->calcForwardKinematics(); @@ -74,9 +74,11 @@ extern "C" { int _getJointAngles (double* ja) { - for ( int i = 0; i < m_robot->numJoints(); i++ ) { + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) { ja[i] = m_robot->joint(i)->q; } + + return 0; } int _calcInverseKinematics2Loop (double* _vel_p, double* _vel_r) diff --git a/rtc/ImpedanceController/ObjectTurnaroundDetector.h b/rtc/ImpedanceController/ObjectTurnaroundDetector.h index 7b9dbc69b9c..998e8cdfbc8 100644 --- a/rtc/ImpedanceController/ObjectTurnaroundDetector.h +++ b/rtc/ImpedanceController/ObjectTurnaroundDetector.h @@ -5,6 +5,8 @@ #include "../TorqueFilter/IIRFilter.h" #include #include +#include +#include "hrpsys/util/Hrpsys.h" class ObjectTurnaroundDetector { @@ -16,7 +18,7 @@ class ObjectTurnaroundDetector boost::shared_ptr > dwrench_filter; hrp::Vector3 axis, moment_center; double prev_wrench, dt; - double detect_ratio_thre, start_ratio_thre, ref_dwrench, max_time, current_time; + double detect_ratio_thre, start_ratio_thre, ref_dwrench, max_time, current_time, current_wrench; size_t count; // detect_count_thre*dt and start_ratio_thre*dt are threshould for time. // detect_count_thre*dt : Threshould for time [s] after the first object turnaround detection (Wait detect_time_thre [s] after first object turnaround detection). @@ -81,6 +83,7 @@ class ObjectTurnaroundDetector dwrench_filter->reset(0); is_dwr_changed = false; } + current_wrench = wrench_value; double tmp_wr = wrench_filter->passFilter(wrench_value); double tmp_dwr = dwrench_filter->passFilter((tmp_wr-prev_wrench)/dt); prev_wrench = tmp_wr; @@ -139,8 +142,8 @@ class ObjectTurnaroundDetector void setDwrenchCutoffFreq (const double a) { dwrench_filter->setCutOffFreq(a); }; void setDetectRatioThre (const double a) { detect_ratio_thre = a; }; void setStartRatioThre (const double a) { start_ratio_thre = a; }; - void setDetectTimeThre (const double a) { detect_count_thre = static_cast(a/dt); }; - void setStartTimeThre (const double a) { start_count_thre = static_cast(a/dt); }; + void setDetectTimeThre (const double a) { detect_count_thre = round(a/dt); }; + void setStartTimeThre (const double a) { start_count_thre = round(a/dt); }; void setAxis (const hrp::Vector3& a) { axis = a; }; void setMomentCenter (const hrp::Vector3& a) { moment_center = a; }; void setDetectorTotalWrench (const detector_total_wrench _dtw) @@ -161,5 +164,6 @@ class ObjectTurnaroundDetector detector_total_wrench getDetectorTotalWrench () const { return dtw; }; double getFilteredWrench () const { return wrench_filter->getCurrentValue(); }; double getFilteredDwrench () const { return dwrench_filter->getCurrentValue(); }; + double getRawWrench () const { return current_wrench; }; }; #endif // OBJECTTURNAROUNDDETECTOR_H diff --git a/rtc/ImpedanceController/RatsMatrix.cpp b/rtc/ImpedanceController/RatsMatrix.cpp index 324500d3b3e..a015f5ea7e9 100644 --- a/rtc/ImpedanceController/RatsMatrix.cpp +++ b/rtc/ImpedanceController/RatsMatrix.cpp @@ -43,10 +43,10 @@ namespace rats ret_dif_rot = self_rot * hrp::Vector3(rats::matrix_log(hrp::Matrix33(self_rot.transpose() * target_rot))); } - void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2) { + void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2, const double eps) { hrp::Matrix33 r(rot1.transpose() * rot2); hrp::Vector3 omega(matrix_log(r)); - if (eps_eq(omega.norm(),0.0)) { // c1.rot and c2.rot are same + if (eps_eq(omega.norm(),0.0, eps)) { // c1.rot and c2.rot are same mid_rot = rot1; } else { hrp::calcRodrigues(r, omega.normalized(), omega.norm()*p); @@ -55,9 +55,9 @@ namespace rats } }; - void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2) { + void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2, const double eps) { mid_coords.pos = (1 - p) * c1.pos + p * c2.pos; - mid_rot(mid_coords.rot, p, c1.rot, c2.rot); + mid_rot(mid_coords.rot, p, c1.rot, c2.rot, eps); }; } diff --git a/rtc/ImpedanceController/RatsMatrix.h b/rtc/ImpedanceController/RatsMatrix.h index 3851fc9cc0b..a7681256251 100644 --- a/rtc/ImpedanceController/RatsMatrix.h +++ b/rtc/ImpedanceController/RatsMatrix.h @@ -91,7 +91,7 @@ namespace rats } }; - void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2); - void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2); + void mid_rot(hrp::Matrix33& mid_rot, const double p, const hrp::Matrix33& rot1, const hrp::Matrix33& rot2, const double eps = 0.001); + void mid_coords(coordinates& mid_coords, const double p, const coordinates& c1, const coordinates& c2, const double eps = 0.001); }; #endif /* RATSMATRIX_H */ diff --git a/rtc/ImpedanceController/testImpedanceOutputGenerator.cpp b/rtc/ImpedanceController/testImpedanceOutputGenerator.cpp index a228934c748..45a6425522e 100644 --- a/rtc/ImpedanceController/testImpedanceOutputGenerator.cpp +++ b/rtc/ImpedanceController/testImpedanceOutputGenerator.cpp @@ -129,7 +129,7 @@ class testImpedanceOutputGenerator }; void parse_params () { - for (int i = 0; i < arg_strs.size(); ++ i) { + for (unsigned int i = 0; i < arg_strs.size(); ++ i) { if ( arg_strs[i]== "--use-gnuplot" ) { if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true"); } diff --git a/rtc/ImpedanceController/testObjectTurnaroundDetector.cpp b/rtc/ImpedanceController/testObjectTurnaroundDetector.cpp index 39f23a0f535..8903ddc2d65 100644 --- a/rtc/ImpedanceController/testObjectTurnaroundDetector.cpp +++ b/rtc/ImpedanceController/testObjectTurnaroundDetector.cpp @@ -80,7 +80,7 @@ class testObjectTurnaroundDetector }; void parse_params () { - for (int i = 0; i < arg_strs.size(); ++ i) { + for (unsigned int i = 0; i < arg_strs.size(); ++ i) { if ( arg_strs[i]== "--use-gnuplot" ) { if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true"); } diff --git a/rtc/Joystick/CMakeLists.txt b/rtc/Joystick/CMakeLists.txt index ffcbbdf2a5c..720eebcdeb6 100644 --- a/rtc/Joystick/CMakeLists.txt +++ b/rtc/Joystick/CMakeLists.txt @@ -16,6 +16,6 @@ target_link_libraries(JoystickComp ${OPENRTM_LIBRARIES} ${js_lib}) set(target Joystick JoystickComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Joystick/Joystick.h b/rtc/Joystick/Joystick.h index 06365b1d615..ffd1d14a250 100644 --- a/rtc/Joystick/Joystick.h +++ b/rtc/Joystick/Joystick.h @@ -10,6 +10,9 @@ #ifndef JOYSTICK_H #define JOYSTICK_H +#include +#include +#include #include #include #include diff --git a/rtc/Joystick2PanTiltAngles/CMakeLists.txt b/rtc/Joystick2PanTiltAngles/CMakeLists.txt index 10dfe21097b..27e6f236a9e 100644 --- a/rtc/Joystick2PanTiltAngles/CMakeLists.txt +++ b/rtc/Joystick2PanTiltAngles/CMakeLists.txt @@ -8,6 +8,6 @@ target_link_libraries(Joystick2PanTiltAnglesComp ${OPENRTM_LIBRARIES}) set(target Joystick2PanTiltAngles Joystick2PanTiltAnglesComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.cpp b/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.cpp index c3d30ae5292..1703323e1ec 100644 --- a/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.cpp +++ b/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.cpp @@ -7,8 +7,8 @@ * $Id$ */ +#include "hrpsys/util/VectorConvert.h" #include "Joystick2PanTiltAngles.h" -#include "util/VectorConvert.h" // Module specification // diff --git a/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.h b/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.h index a1e6b58e746..2bbefc1ca19 100644 --- a/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.h +++ b/rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.h @@ -10,6 +10,8 @@ #ifndef JOYSTICK2PANTILTANGLES_H #define JOYSTICK2PANTILTANGLES_H +#include +#include #include #include #include diff --git a/rtc/Joystick2Velocity2D/CMakeLists.txt b/rtc/Joystick2Velocity2D/CMakeLists.txt index 098698b66f0..e47116068a8 100644 --- a/rtc/Joystick2Velocity2D/CMakeLists.txt +++ b/rtc/Joystick2Velocity2D/CMakeLists.txt @@ -8,6 +8,6 @@ target_link_libraries(Joystick2Velocity2DComp ${OPENRTM_LIBRARIES}) set(target Joystick2Velocity2D Joystick2Velocity2DComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Joystick2Velocity2D/Joystick2Velocity2D.cpp b/rtc/Joystick2Velocity2D/Joystick2Velocity2D.cpp index 29f7a2e9eee..6c526f095f4 100644 --- a/rtc/Joystick2Velocity2D/Joystick2Velocity2D.cpp +++ b/rtc/Joystick2Velocity2D/Joystick2Velocity2D.cpp @@ -7,8 +7,10 @@ * $Id$ */ +#include +#include +#include "hrpsys/util/VectorConvert.h" #include "Joystick2Velocity2D.h" -#include "util/VectorConvert.h" // Module specification // diff --git a/rtc/Joystick2Velocity2D/Joystick2Velocity2D.h b/rtc/Joystick2Velocity2D/Joystick2Velocity2D.h index eddd9aa906e..bb1181846f9 100644 --- a/rtc/Joystick2Velocity2D/Joystick2Velocity2D.h +++ b/rtc/Joystick2Velocity2D/Joystick2Velocity2D.h @@ -10,6 +10,8 @@ #ifndef JOYSTICK2VELOCITY_H #define JOYSTICK2VELOCITY_H +#include +#include #include #include #include diff --git a/rtc/Joystick2Velocity3D/CMakeLists.txt b/rtc/Joystick2Velocity3D/CMakeLists.txt index aeef535b96b..20deba456cc 100644 --- a/rtc/Joystick2Velocity3D/CMakeLists.txt +++ b/rtc/Joystick2Velocity3D/CMakeLists.txt @@ -8,6 +8,6 @@ target_link_libraries(Joystick2Velocity3DComp ${OPENRTM_LIBRARIES}) set(target Joystick2Velocity3D Joystick2Velocity3DComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Joystick2Velocity3D/Joystick2Velocity3D.cpp b/rtc/Joystick2Velocity3D/Joystick2Velocity3D.cpp index d7c71c2cbfa..d12134b92f0 100644 --- a/rtc/Joystick2Velocity3D/Joystick2Velocity3D.cpp +++ b/rtc/Joystick2Velocity3D/Joystick2Velocity3D.cpp @@ -7,8 +7,10 @@ * $Id$ */ +#include +#include +#include "hrpsys/util/VectorConvert.h" #include "Joystick2Velocity3D.h" -#include "util/VectorConvert.h" // Module specification // diff --git a/rtc/Joystick2Velocity3D/Joystick2Velocity3D.h b/rtc/Joystick2Velocity3D/Joystick2Velocity3D.h index 255e98563bb..8377c1dbf0b 100644 --- a/rtc/Joystick2Velocity3D/Joystick2Velocity3D.h +++ b/rtc/Joystick2Velocity3D/Joystick2Velocity3D.h @@ -10,6 +10,8 @@ #ifndef JOYSTICK2VELOCITY_H #define JOYSTICK2VELOCITY_H +#include +#include #include #include #include diff --git a/rtc/JpegDecoder/CMakeLists.txt b/rtc/JpegDecoder/CMakeLists.txt index 1d7cfe5f649..9125269c17b 100644 --- a/rtc/JpegDecoder/CMakeLists.txt +++ b/rtc/JpegDecoder/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(JpegDecoderComp ${libs}) set(target JpegDecoder JpegDecoderComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/JpegDecoder/JpegDecoder.cpp b/rtc/JpegDecoder/JpegDecoder.cpp index 2eefc279f1e..c67b0a4ee18 100644 --- a/rtc/JpegDecoder/JpegDecoder.cpp +++ b/rtc/JpegDecoder/JpegDecoder.cpp @@ -7,8 +7,8 @@ * $Id$ */ -#include -#include +#include +#include #include "JpegDecoder.h" // Module specification diff --git a/rtc/JpegDecoder/JpegDecoder.h b/rtc/JpegDecoder/JpegDecoder.h index cccb523e310..87b3b9e5d47 100644 --- a/rtc/JpegDecoder/JpegDecoder.h +++ b/rtc/JpegDecoder/JpegDecoder.h @@ -10,13 +10,14 @@ #ifndef JPEG_DECODER_H #define JPEG_DECODER_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/JpegEncoder/CMakeLists.txt b/rtc/JpegEncoder/CMakeLists.txt index b0715a8712c..2d7a480679c 100644 --- a/rtc/JpegEncoder/CMakeLists.txt +++ b/rtc/JpegEncoder/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(JpegEncoderComp ${libs}) set(target JpegEncoder JpegEncoderComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/JpegEncoder/JpegEncoder.cpp b/rtc/JpegEncoder/JpegEncoder.cpp index 68b2a492f01..cf9fd0a69ee 100644 --- a/rtc/JpegEncoder/JpegEncoder.cpp +++ b/rtc/JpegEncoder/JpegEncoder.cpp @@ -7,8 +7,9 @@ * $Id$ */ -#include -#include +#include +#include +#include #include "JpegEncoder.h" // Module specification @@ -131,14 +132,14 @@ RTC::ReturnCode_t JpegEncoder::onExecute(RTC::UniqueId ec_id) { // RGB -> BGR uchar r,g,b, *raw=idat.raw_data.get_buffer(); - for (int i=0; i +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/KalmanFilter/CMakeLists.txt b/rtc/KalmanFilter/CMakeLists.txt index 92be2d3ef19..638b729aa2f 100644 --- a/rtc/KalmanFilter/CMakeLists.txt +++ b/rtc/KalmanFilter/CMakeLists.txt @@ -15,12 +15,14 @@ set_target_properties(KalmanFilter PROPERTIES PREFIX "") add_executable(KalmanFilterComp KalmanFilterComp.cpp ${comp_sources}) target_link_libraries(KalmanFilterComp ${libs}) +add_executable(testKFilter testKFilter.cpp) +target_link_libraries(testKFilter ${libs}) add_executable(testKalmanFilterEstimation testKalmanFilterEstimation.cpp) target_link_libraries(testKalmanFilterEstimation ${libs}) -set(target KalmanFilter KalmanFilterComp testKalmanFilterEstimation) +set(target KalmanFilter KalmanFilterComp testKalmanFilterEstimation testKFilter) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/KalmanFilter/EKFilter.h b/rtc/KalmanFilter/EKFilter.h index 8927f62c28a..a6b34c80c85 100644 --- a/rtc/KalmanFilter/EKFilter.h +++ b/rtc/KalmanFilter/EKFilter.h @@ -1,204 +1,188 @@ +// -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*- #ifndef EKFILTER_H #define EKFILTER_H #include #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" + +namespace hrp{ + typedef Eigen::Matrix Matrix77; + typedef Eigen::Matrix Vector7; +}; class EKFilter { public: - EKFilter() { + EKFilter() + : P(hrp::Matrix77::Identity() * 0.1), + Q(Eigen::Matrix3d::Identity() * 0.001), + R(Eigen::Matrix3d::Identity() * 0.03), + g_vec(Eigen::Vector3d(0.0, 0.0, 9.80665)), + z_k(Eigen::Vector3d(0.0, 0.0, 9.80665)), + min_mag_thre_acc(0.005), max_mag_thre_acc(0.05), + min_mag_thre_gyro(0.0075), max_mag_thre_gyro(0.035) + { x << 1, 0, 0, 0, 0, 0, 0; - P = Eigen::Matrix::Identity() * 5; - Q = Eigen::Matrix::Identity(); - Q.block<4, 4>(0, 0) *= 0.1; - Q.block<3, 3>(4, 4) *= 0.001; - R = Eigen::Matrix::Zero(); - R(0, 0) = 4; - R(1, 1) = 4; - R(2, 2) = 2; - g_vec = Eigen::Vector3d(0.0, 0.0, 9.80665); } - Eigen::Matrix getx() { return x; } - - Eigen::Matrix accelerationToRpy(const double& acc_x, const double& acc_y, const double& acc_z) { - /* - * acc = \dot{v} + w \times v + R^T g; - * -> R^T g = acc - \dot{v} + w \times v; - * -> R^T g = acc; - * -> acc_x = -sinb g, acc_y = sina cosb g, acc_z = cosa cosb g; - */ - double roll = atan2(acc_y, acc_z); - double pitch = atan2(-acc_x, sqrt(acc_y * acc_y + acc_z * acc_z)); - double yaw = 0; /* cannot be defined only by acceleration */ - Eigen::Vector3d rpy = Eigen::Vector3d(roll, pitch, yaw); - return rpy; - } + hrp::Vector7 getx() const { return x; } - Eigen::Matrix calcOmega(const Eigen::Vector3d& w) { + void calcOmega(Eigen::Matrix4d& omega, const Eigen::Vector3d& w) const { /* \dot{q} = \frac{1}{2} omega q */ - Eigen::Matrix omega; omega << 0, -w[0], -w[1], -w[2], w[0], 0, w[2], -w[1], w[1], -w[2], 0, w[0], w[2], w[1], -w[0], 0; - return omega; } - Eigen::Matrix calcPredictedState(Eigen::Matrix q, - Eigen::Vector3d gyro, - Eigen::Vector3d drift, - const double& dt) { + void calcPredictedState(hrp::Vector7& _x_a_priori, + const Eigen::Vector4d& q, + const Eigen::Vector3d& gyro, + const Eigen::Vector3d& drift) const { /* x_a_priori = f(x, u) */ - Eigen::Matrix ret; - Eigen::Matrix q_a_priori; + Eigen::Vector4d q_a_priori; Eigen::Vector3d gyro_compensated = gyro - drift; - q_a_priori = q + dt / 2 * calcOmega(gyro_compensated) * q; - ret.block<4, 1>(0, 0) = q_a_priori; - ret.block<3, 1>(4, 0) = drift; - return ret; + Eigen::Matrix4d omega; + calcOmega(omega, gyro_compensated); + q_a_priori = q + dt / 2 * omega * q; + _x_a_priori.head<4>() = q_a_priori.normalized(); + _x_a_priori.tail<3>() = drift; } - Eigen::Matrix calcF(Eigen::Matrix q, - const Eigen::Vector3d& gyro, - Eigen::Vector3d drift, - const double& dt) { - Eigen::Matrix F; + void calcF(hrp::Matrix77& F, + const Eigen::Vector4d& q, + const Eigen::Vector3d& gyro, + const Eigen::Vector3d& drift) const { + F = hrp::Matrix77::Identity(); Eigen::Vector3d gyro_compensated = gyro - drift; - F.block<4, 4>(0, 0) = - Eigen::Matrix::Identity() + dt / 2 * calcOmega(gyro_compensated); + Eigen::Matrix4d omega; + calcOmega(omega, gyro_compensated); + F.block<4, 4>(0, 0) += dt / 2 * omega; F.block<4, 3>(0, 4) << - dt / 2 * q[1], dt / 2 * q[2], dt / 2 * q[3], - - dt / 2 * q[0], dt / 2 * q[3], - dt / 2 * q[2], - - dt / 2 * q[3], - dt / 2 * q[0], dt / 2 * q[1], - dt / 2 * q[2], - dt / 2 * q[1], - dt / 2 * q[0]; - F.block<3, 4>(0, 4) = Eigen::Matrix::Zero(); - F.block<3, 3>(4, 4) = Eigen::Matrix::Identity(); - return F; + + q[1], + q[2], + q[3], + - q[0], + q[3], - q[2], + - q[3], - q[0], + q[1], + + q[2], - q[1], - q[0]; + F.block<4, 3>(0, 4) *= dt / 2; } - Eigen::Matrix calcPredictedCovariance(Eigen::Matrix F) { - /* P_a_priori = F P F^T + Q */ - return F * P * F.transpose() + Q; + void calcPredictedCovariance(hrp::Matrix77& _P_a_priori, + const hrp::Matrix77& F, + const Eigen::Vector4d& q) const { + /* P_a_priori = F P F^T + V Q V^T */ + Eigen::Matrix V_upper; + V_upper << + - q[1], - q[2], - q[3], + + q[0], - q[3], + q[2], + + q[3], + q[0], - q[1], + - q[2], + q[1], + q[0]; + V_upper *= dt / 2; + hrp::Matrix77 VQVt = hrp::Matrix77::Zero(); + VQVt.block<4, 4>(0, 0) = V_upper * Q * V_upper.transpose(); + _P_a_priori = F * P * F.transpose() + VQVt; } - Eigen::Vector3d calcAcc(Eigen::Matrix q, - const Eigen::Vector3d& vel_ref, - const Eigen::Vector3d& acc_ref, - const Eigen::Vector3d& angular_rate_ref) { - /* acc = \dot{v} + w \times v + R^T g; */ - Eigen::Quaternion q_tmp = Eigen::Quaternion(q[0], q[1], q[2], q[3]); - Eigen::Vector3d acc = - acc_ref + angular_rate_ref.cross(vel_ref) + - q_tmp.toRotationMatrix().transpose() * g_vec; - /* - * Eigen::Vector3d hoge; - * hoge << - * -2 * q_tmp2.w() * q_tmp2.y() + 2 * q_tmp2.x() * q_tmp2.z(), - * 2 * q_tmp2.w() * q_tmp2.x() + 2 * q_tmp2.y() * q_tmp2.z(), - * q_tmp2.w() * q_tmp2.w() - q_tmp2.x() * q_tmp2.x() - q_tmp2.y() * q_tmp2.y() + q_tmp2.z() * q_tmp2.z(); - * hoge *= g_vec[2]; - * std::cerr << "diff 2" << std::endl << acc - hoge << std::endl; - */ + Eigen::Vector3d calcAcc(const Eigen::Vector4d& q) const { + Eigen::Quaternion q_tmp(q[0], q[1], q[2], q[3]); + Eigen::Vector3d acc = q_tmp.conjugate()._transformVector(g_vec); return acc; } - Eigen::Matrix calcH(Eigen::Matrix q) { - Eigen::Matrix H; + void calcH(Eigen::Matrix& H, const Eigen::Vector4d& q) const { double w = q[0], x = q[1], y = q[2], z = q[3]; - /* - * H << - * 2 * y, 2 * z, 2 * w, 2 * x, 0, 0, 0, - * -2 * x, -2 * w, 2 * z, 2 * y, 0, 0, 0, - * 2 * w, -2 * x, -2 * y, 2 * z, 0, 0, 0; - * H *= g_vec[2]; - */ H << - -y, z, -w, x, 0, 0, 0, - x, w, z, y, 0, 0, 0, - w, -x, -y, z, 0, 0, 0; + -y, +z, -w, +x, 0, 0, 0, + +x, +w, +z, +y, 0, 0, 0, + +w, -x, -y, +z, 0, 0, 0; H *= 2 * g_vec[2]; - return H; } Eigen::Vector3d calcMeasurementResidual(const Eigen::Vector3d& acc_measured, - const Eigen::Vector3d& vel_ref, - const Eigen::Vector3d& acc_ref, - const Eigen::Vector3d& angular_rate_ref, - Eigen::Matrix q) { + const Eigen::Vector4d& q) const { /* y = z - h(x) */ - Eigen::Vector3d y = acc_measured - calcAcc(q, vel_ref, acc_ref, angular_rate_ref); - /* - * std::cerr << "acc_measured" << std::endl << acc_measured << std::endl; - * std::cerr << "calc acc" << std::endl << calcAcc(q, vel_ref, acc_ref, angular_rate_ref) << std::endl; - * std::cerr << "diff" << std::endl << y << std::endl; - */ + Eigen::Vector3d y = acc_measured - calcAcc(q); return y; } void prediction(const Eigen::Vector3d& u) { - Eigen::Matrix q = x.block<4, 1>(0, 0); - Eigen::Vector3d drift = x.block<3, 1>(4, 0); - Eigen::Matrix F = calcF(q, u, drift, dt); - x_a_priori = calcPredictedState(q, u, drift, dt); - P_a_priori = calcPredictedCovariance(F); + Eigen::Vector4d q = x.head<4>(); + Eigen::Vector3d drift = x.tail<3>(); + hrp::Matrix77 F; + calcF(F, q, u, drift); + hrp::Vector7 x_tmp; + calcPredictedState(x_tmp, q, u, drift); + x_a_priori = x_tmp; + hrp::Matrix77 P_tmp; + calcPredictedCovariance(P_tmp, F, q); + P_a_priori = P_tmp; } - void correction(const Eigen::Vector3d& z, - const Eigen::Vector3d& vel_ref, - const Eigen::Vector3d& acc_ref, - const Eigen::Vector3d& angular_rate_ref) { - Eigen::Matrix q_a_priori = x_a_priori.block<4, 1>(0, 0).normalized(); + void correction(const Eigen::Vector3d& z, const Eigen::Matrix3d& fuzzyR) { + Eigen::Vector4d q_a_priori = x_a_priori.head<4>(); Eigen::Matrix H; - Eigen::Matrix S; - Eigen::Matrix K; - Eigen::Vector3d y; - /* need to normalize q_a_priori ? */ - y = calcMeasurementResidual(z, vel_ref, acc_ref, angular_rate_ref, q_a_priori); - H = calcH(q_a_priori); - S = H * P_a_priori * H.transpose() + R; - K = P_a_priori * H.transpose() * S.inverse(); - Eigen::Matrix x_tmp = x_a_priori + K * y; - x = x_tmp.normalized(); - P = (Eigen::Matrix::Identity() - K * H) * P_a_priori; + z_k = z; + Eigen::Vector3d y = calcMeasurementResidual(z, q_a_priori); + calcH(H, q_a_priori); + Eigen::Matrix3d S = H * P_a_priori * H.transpose() + fuzzyR; + Eigen::Matrix K = P_a_priori * H.transpose() * S.inverse(); + hrp::Vector7 x_tmp = x_a_priori + K * y; + x.head<4>() = x_tmp.head<4>().normalized(); /* quaternion */ + x.tail<3>() = x_tmp.tail<3>(); /* bias */ + P = (hrp::Matrix77::Identity() - K * H) * P_a_priori; } - void printAll() { + void printAll() const { std::cerr << "x" << std::endl << x << std::endl; std::cerr << "x_a_priori" << std::endl << x_a_priori << std::endl; std::cerr << "P" << std::endl << P << std::endl << std::endl; std::cerr << "P_a_priori" << std::endl << P_a_priori << std::endl << std::endl; - /* - * std::cerr << "Q" << std::endl << Q << std::endl << std::endl; - * std::cerr << "R" << std::endl << R << std::endl << std::endl; - */ } + + // Basically Equation (23), (24) and (25) in the paper [1] + // [1] Chul Woo Kang and Chan Gook Park. Attitude estimation with accelerometers and gyros using fuzzy tuned Kalman filter. + // In European Control Conference, 2009. + void calcRWithFuzzyRule(Eigen::Matrix3d& fuzzyR, const hrp::Vector3& acc, const hrp::Vector3& gyro) const { + double alpha = std::min(std::fabs(acc.norm() - g_vec.norm()) / g_vec.norm(), 0.1); + double beta = std::min(gyro.norm(), 0.05); + double large_mu_acc = std::max(std::min((alpha - min_mag_thre_acc) / (max_mag_thre_acc - min_mag_thre_acc), 1.0), 0.0); + double large_mu_gyro = std::max(std::min((beta - min_mag_thre_gyro) / (max_mag_thre_gyro - min_mag_thre_gyro), 1.0), 0.0); + double w1, w2, w3, w4; + w1 = (1.0 - large_mu_acc) * (1.0 - large_mu_gyro); + w2 = (1.0 - large_mu_acc) * large_mu_gyro; + w3 = large_mu_acc * (1.0 - large_mu_gyro); + w4 = large_mu_acc * large_mu_gyro; + double z = (w1 * 0.0 + w2 * (3.5 * alpha + 8.0 * beta + 0.5) + w3 * (3.5 * alpha + 8.0 * beta + 0.5) + w4 * 1.0) / (w1 + w2 + w3 + w4); + double k1 = 400; + fuzzyR = R + k1 * z * z * Eigen::Matrix3d::Identity(); + }; + void main_one (hrp::Vector3& rpy, hrp::Vector3& rpyRaw, const hrp::Vector3& acc, const hrp::Vector3& gyro) { - prediction(gyro); - correction(acc, Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0), Eigen::Vector3d(0, 0, 0)); - /* ekf_filter.printAll(); */ - Eigen::Matrix x = getx(); - Eigen::Quaternion q = Eigen::Quaternion(x[0], x[1], x[2], x[3]); - hrp::Vector3 eulerZYX = q.toRotationMatrix().eulerAngles(2,1,0); - rpy(2) = eulerZYX(0); - rpy(1) = eulerZYX(1); - rpy(0) = eulerZYX(2); + Eigen::Matrix3d fuzzyR; + calcRWithFuzzyRule(fuzzyR, acc, gyro); + prediction(gyro); + correction(acc, fuzzyR); + /* ekf_filter.printAll(); */ + Eigen::Quaternion q(x[0], x[1], x[2], x[3]); + rpy = hrp::rpyFromRot(q.toRotationMatrix()); }; void setdt (const double _dt) { dt = _dt;}; + void resetKalmanFilterState() { + Eigen::Quaternion tmp_q; + tmp_q.setFromTwoVectors(z_k, g_vec); + x << tmp_q.w(), tmp_q.x(), tmp_q.y(), tmp_q.z(), 0, 0, 0; + }; private: - Eigen::Matrix x, x_a_priori; - Eigen::Matrix P, P_a_priori; - Eigen::Matrix Q; - Eigen::Matrix R; - /* static const Eigen::Vector3d g_vec = Eigen::Vector3d(0.0, 0.0, 9.80665); */ - Eigen::Vector3d g_vec; + hrp::Vector7 x, x_a_priori; + hrp::Matrix77 P, P_a_priori; + Eigen::Matrix3d Q, R; + Eigen::Vector3d g_vec, z_k; double dt; + double min_mag_thre_acc, max_mag_thre_acc, min_mag_thre_gyro, max_mag_thre_gyro; }; #endif /* EKFILTER_H */ diff --git a/rtc/KalmanFilter/KalmanFilter.cpp b/rtc/KalmanFilter/KalmanFilter.cpp index 9a871a1d1e6..873fd47b35d 100644 --- a/rtc/KalmanFilter/KalmanFilter.cpp +++ b/rtc/KalmanFilter/KalmanFilter.cpp @@ -8,7 +8,7 @@ */ #include "KalmanFilter.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include #include #include @@ -210,6 +210,7 @@ RTC::ReturnCode_t KalmanFilter::onExecute(RTC::UniqueId ec_id) Eigen::Vector3d acc = m_sensorR * hrp::Vector3(m_acc.data.ax-sx_ref+acc_offset(0), m_acc.data.ay-sy_ref+acc_offset(1), m_acc.data.az-sz_ref+acc_offset(2)); // transform to imaginary acc data acc = sensorR_offset * acc; Eigen::Vector3d gyro = m_sensorR * hrp::Vector3(m_rate.data.avx, m_rate.data.avy, m_rate.data.avz); // transform to imaginary rate data + gyro = sensorR_offset * gyro; if (DEBUGP) { std::cerr << "[" << m_profile.instance_name << "] raw data acc : " << std::endl << acc << std::endl; std::cerr << "[" << m_profile.instance_name << "] raw data gyro : " << std::endl << gyro << std::endl; @@ -309,6 +310,7 @@ bool KalmanFilter::setKalmanFilterParam(const OpenHRP::KalmanFilterService::Kalm bool KalmanFilter::resetKalmanFilterState() { rpy_kf.resetKalmanFilterState(); + ekf_filter.resetKalmanFilterState(); }; bool KalmanFilter::getKalmanFilterParam(OpenHRP::KalmanFilterService::KalmanFilterParam& i_param) diff --git a/rtc/KalmanFilter/KalmanFilter.h b/rtc/KalmanFilter/KalmanFilter.h index 7b480407a6f..3d8b0279d51 100644 --- a/rtc/KalmanFilter/KalmanFilter.h +++ b/rtc/KalmanFilter/KalmanFilter.h @@ -10,6 +10,8 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H +#include +#include #include #include #include diff --git a/rtc/KalmanFilter/KalmanFilterService_impl.h b/rtc/KalmanFilter/KalmanFilterService_impl.h index 645205d2d20..48aef9a3fdf 100644 --- a/rtc/KalmanFilter/KalmanFilterService_impl.h +++ b/rtc/KalmanFilter/KalmanFilterService_impl.h @@ -2,7 +2,7 @@ #ifndef __KALMANFILTER_SERVICE_H__ #define __KALMANFILTER_SERVICE_H__ -#include "KalmanFilterService.hh" +#include "hrpsys/idl/KalmanFilterService.hh" class KalmanFilter; diff --git a/rtc/KalmanFilter/RPYKalmanFilter.h b/rtc/KalmanFilter/RPYKalmanFilter.h index ad17463df11..4c3967af8be 100644 --- a/rtc/KalmanFilter/RPYKalmanFilter.h +++ b/rtc/KalmanFilter/RPYKalmanFilter.h @@ -3,7 +3,7 @@ #include #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" namespace hrp{ typedef Eigen::Vector2d Vector2; typedef Eigen::Matrix2d Matrix22; diff --git a/rtc/KalmanFilter/testKFilter.cpp b/rtc/KalmanFilter/testKFilter.cpp new file mode 100644 index 00000000000..eab3982d76e --- /dev/null +++ b/rtc/KalmanFilter/testKFilter.cpp @@ -0,0 +1,101 @@ +// -*- C++ -*- +/*! + * @file testKalmanFilter.cpp + * @brief Test program for KalmanFilter.cpp requires input log data files + * @date $Date$ + * + * $Id$ + */ + + +#include +#include +#include "RPYKalmanFilter.h" + +// Usage +// testKalmanFilter --acc-file /tmp/kftest/test.acc --rate-file /tmp/kftest/test.rate --pose-file /tmp/kftest/test.pose --Q-angle 1e-3 --Q-rate 1e-10 --dt 0.005 --R-angle 1 --use-gnuplot true + +int main(int argc, char *argv[]) +{ + // Default file names and params + double Q_pos = 0.001; + double Q_vel = 0.003; + double R_pos = 0.005; + double dt = 0.004; + std::string input_file("test.dat"); + bool use_gnuplot = false; + + // Parse argument + for (int i = 0; i < argc; ++ i) { + std::string arg(argv[i]); + if ( arg == "--Q-pos" ) { // KF parameters + if (++i < argc) Q_pos = atof(argv[i]); + } else if ( arg == "--Q-vel" ) { + if (++i < argc) Q_vel = atof(argv[i]); + } else if ( arg == "--R-pos" ) { + if (++i < argc) R_pos = atof(argv[i]); + } else if ( arg == "--dt" ) { // sampling time[s] + if (++i < argc) dt = atof(argv[i]); + } else if ( arg == "--input-file" ) { // File path for rate + if (++i < argc) input_file = argv[i]; + } else if ( arg == "--use-gnuplot" ) { // Use gnuplot (true or false) + if (++i < argc) use_gnuplot = (std::string(argv[i])=="true"?true:false); + } + } + + // Setup input and output files + std::ifstream inputf(input_file.c_str()); + std::cerr << "File : " << input_file << std::endl; + if (!inputf.is_open()) { + std::cerr << "No such " << input_file << std::endl; + return -1; + } + std::string ofname("/tmp/testKalmanFilter.dat"); + std::ofstream ofs(ofname.c_str()); + + // Test kalman filter + KFilter kf; + //kf.setF(1, -dt, 0, 1); + //kf.setB(dt, 0); + kf.setQ(Q_pos*dt, 0, 0, Q_vel*dt); + kf.setB(0, 0); + kf.setF(1, dt, 0, 1); + kf.setP(0, 0, 0, 0); + kf.setR(R_pos); + //hrp::Vector3 rate, acc, rpy, rpyRaw, baseRpyCurrent, rpyAct; + double time, time2=0.0; + double data; + while(!inputf.eof()){ + inputf >> time >> data; + kf.update(0, data); + if (use_gnuplot) { + ofs << time2 << " " << data << " " << kf.getx()[0] << " " << kf.getx()[1] << std::endl; + } else { + std::cout << data << std::endl; + } + time2+=dt; + } + // gnuplot + if (use_gnuplot) { + FILE* gp[2]; + std::string titles[2] = {"Pos", "Vel"}; + for (size_t ii = 0; ii < 2; ii++) { + gp[ii] = popen("gnuplot", "w"); + fprintf(gp[ii], "set title \"%s\"\n", titles[ii].c_str()); + fprintf(gp[ii], "set xlabel \"Time [s]\"\n"); + fprintf(gp[ii], "set ylabel \"Pos\"\n"); + if (ii==0) { + fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Filtered\"\n", ofname.c_str(), 2); + fprintf(gp[ii], "replot \"%s\" using 1:%d with lines title \"Raw\"\n", ofname.c_str(), 3); + } else { + fprintf(gp[ii], "plot \"%s\" using 1:%d with lines title \"Vel\"\n", ofname.c_str(), 4); + } + fflush(gp[ii]); + } + std::cout << "Type any keys + enter to exit." << std::endl; + double tmp; + std::cin >> tmp; + for (size_t j = 0; j < 2; j++) pclose(gp[j]); + } + return 0; +} diff --git a/rtc/MLSFilter/CMakeLists.txt b/rtc/MLSFilter/CMakeLists.txt index 9ca50bffef1..940491717ed 100644 --- a/rtc/MLSFilter/CMakeLists.txt +++ b/rtc/MLSFilter/CMakeLists.txt @@ -14,6 +14,6 @@ target_link_libraries(MLSFilterComp ${libs}) set(target MLSFilter MLSFilterComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/MLSFilter/MLSFilter.cpp b/rtc/MLSFilter/MLSFilter.cpp index ac7bee02ed5..09b68ece4cf 100644 --- a/rtc/MLSFilter/MLSFilter.cpp +++ b/rtc/MLSFilter/MLSFilter.cpp @@ -11,7 +11,7 @@ #include #include #include "MLSFilter.h" -#include "pointcloud.hh" +#include "hrpsys/idl/pointcloud.hh" // Module specification // @@ -147,7 +147,7 @@ RTC::ReturnCode_t MLSFilter::onExecute(RTC::UniqueId ec_id) // RTM -> PCL cloud->points.resize(m_original.width*m_original.height); float *src = (float *)m_original.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ cloud->points[i].x = src[0]; cloud->points[i].y = src[1]; cloud->points[i].z = src[2]; @@ -168,7 +168,7 @@ RTC::ReturnCode_t MLSFilter::onExecute(RTC::UniqueId ec_id) m_filtered.row_step = m_filtered.point_step*m_filtered.width; m_filtered.data.length(m_filtered.height*m_filtered.row_step); float *dst = (float *)m_filtered.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ dst[0] = cloud_filtered->points[i].x; dst[1] = cloud_filtered->points[i].y; dst[2] = cloud_filtered->points[i].z; diff --git a/rtc/MLSFilter/MLSFilter.h b/rtc/MLSFilter/MLSFilter.h index e8e18e7ec2d..7c9cfc1fdc7 100644 --- a/rtc/MLSFilter/MLSFilter.h +++ b/rtc/MLSFilter/MLSFilter.h @@ -10,13 +10,14 @@ #ifndef MOVING_LEAST_SQUARES_FILTER_H #define MOVING_LEAST_SQUARES_FILTER_H +#include +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include "pointcloud.hh" // Service implementation headers // diff --git a/rtc/ModifiedServo/CMakeLists.txt b/rtc/ModifiedServo/CMakeLists.txt new file mode 100644 index 00000000000..13859e94657 --- /dev/null +++ b/rtc/ModifiedServo/CMakeLists.txt @@ -0,0 +1,15 @@ +set(comp_sources ModifiedServo.cpp) +add_library(ModifiedServo SHARED ${comp_sources}) +set(libs hrpModel-3.1 ${OPENRTM_LIBRARIES}) +target_link_libraries(ModifiedServo ${libs}) +set_target_properties(ModifiedServo PROPERTIES PREFIX "") + +add_executable(ModifiedServoComp ModifiedServoComp.cpp ${comp_sources}) +target_link_libraries(ModifiedServoComp ${libs}) + +set(target ModifiedServo ModifiedServoComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/ModifiedServo/ModifiedServo.cpp b/rtc/ModifiedServo/ModifiedServo.cpp new file mode 100644 index 00000000000..e317615375f --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo.cpp @@ -0,0 +1,286 @@ +// -*- C++ -*- +/*! + * @file ModifiedServo.cpp + * @brief ModifiedServo component + * $Date$ + * + * $Id$ + */ + +#include "ModifiedServo.h" + +// Module specification +// +static const char* modifiedservo_spec[] = + { + "implementation_id", "ModifiedServo", + "type_name", "ModifiedServo", + "description", "ModifiedServo component", + "version", "0.1", + "vendor", "AIST", + "category", "example", + "activity_type", "SPORADIC", + "kind", "DataFlowComponent", + "max_instance", "1", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "" + }; +// + +ModifiedServo::ModifiedServo(RTC::Manager* manager) + // + : RTC::DataFlowComponentBase(manager), + m_tauRefIn("tauRef", m_tauRef), + m_qRefIn("qRef", m_qRef), + m_qIn("q", m_q), + m_torqueModeIn("torqueMode", m_torqueMode), + m_tauOut("tau", m_tau), + // + gain_fname(""), + dt(0.005), + dof(0) +{ +} + +ModifiedServo::~ModifiedServo() +{ +} + + +RTC::ReturnCode_t ModifiedServo::onInitialize() +{ + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("tauRef", m_tauRefIn); + addInPort("qRef", m_qRefIn); + addInPort("q", m_qIn); + addInPort("torqueMode", m_torqueModeIn); + + // Set OutPort buffer + addOutPort("tau", m_tauOut); + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + // + // Bind variables and configuration variable + + // + + std::cout << m_profile.instance_name << ": onInitialize() " << std::endl; + + RTC::Properties & prop = getProperties(); + + coil::stringTo(dt, prop["dt"].c_str()); + coil::stringTo(ref_dt, prop["ref_dt"].c_str()); + nstep = ref_dt/dt; + step = nstep; + + m_robot = hrp::BodyPtr(new hrp::Body()); + + RTC::Manager & rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + + if (comPos < 0) + comPos = nameServer.length(); + + // In case there is more than one, retrieves only the first one + nameServer = nameServer.substr(0, comPos); + + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()))) + std::cerr << "[" << m_profile.instance_name << "] failed to load model " + << "[" << prop["model"] << "]" << std::endl; + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ModifiedServo::onFinalize() +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ModifiedServo::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Activated" << std::endl; + + if (m_qIn.isNew()) { + m_qIn.read(); + if (dof == 0) { + dof = m_q.data.length(); + readGainFile(); + } + } + + q_old.resize(dof); + qRef_old.resize(dof); + + m_tauRef.data.length(dof); + m_qRef.data.length(dof); + m_torqueMode.data.length(dof); + + m_tau.data.length(dof); + + for (size_t i = 0; i < dof; i++) { + m_tauRef.data[i] = 0.0; + m_qRef.data[i] = qRef_old[i] = q_old[i] = m_q.data[i]; + m_torqueMode.data[i] = false; + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name << ": on Deactivated" << std::endl; + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ModifiedServo::onExecute(RTC::UniqueId ec_id) +{ + if (m_tauRefIn.isNew()) + m_tauRefIn.read(); + + if (m_qIn.isNew()) + m_qIn.read(); + + if (m_qRefIn.isNew()) { + m_qRefIn.read(); + step = nstep; + } + + if (m_torqueModeIn.isNew()) + m_torqueModeIn.read(); + + for (size_t i = 0; i < dof; i++) { + + double q = m_q.data[i]; + double qRef = step > 0 ? qRef_old[i] + (m_qRef.data[i] - qRef_old[i]) / step : qRef_old[i]; + + double dq = (q - q_old[i]) / dt; + double dqRef = (qRef - qRef_old[i]) / dt; + + q_old[i] = q; + qRef_old[i] = qRef; + + double tau = m_torqueMode.data[i] ? m_tauRef.data[i] : Pgain[i] * (qRef - q) + Dgain[i] * (dqRef - dq); + + double tau_limit = m_robot->joint(i)->torqueConst * m_robot->joint(i)->climit * fabs(m_robot->joint(i)->gearRatio); + + m_tau.data[i] = std::max(std::min(tau, tau_limit), -tau_limit); + } + + step--; + + m_tau.tm = m_q.tm; + m_tauOut.write(); + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ModifiedServo::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ +/* +RTC::ReturnCode_t ModifiedServo::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +void ModifiedServo::readGainFile() +{ + if (gain_fname == "") { + RTC::Properties & prop = getProperties(); + coil::stringTo(gain_fname, prop["pdgains_sim_file_name"].c_str()); + } + + gain.open(gain_fname.c_str()); + + if (gain.is_open()) { + + double val; + + Pgain.resize(dof); + Dgain.resize(dof); + + for (unsigned int i = 0; i < dof; i++) { + + if (gain >> val) + Pgain[i] = val; + else + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + + if (gain >> val) + Dgain[i] = val; + else + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] is too short" << std::endl; + } + + gain.close(); + + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] successfully read" << std::endl; + } + else + std::cout << "[" << m_profile.instance_name << "] Gain file [" << gain_fname << "] could not be opened" << std::endl; +} + +extern "C" +{ + + void ModifiedServoInit(RTC::Manager* manager) + { + coil::Properties profile(modifiedservo_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; diff --git a/rtc/ModifiedServo/ModifiedServo.h b/rtc/ModifiedServo/ModifiedServo.h new file mode 100644 index 00000000000..a58ed0863a0 --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServo.h @@ -0,0 +1,161 @@ +// -*- C++ -*- +/*! + * @file ModifiedServo.h + * @brief ModifiedServo component + * @date $Date$ + * + * $Id$ + */ + +#ifndef MODIFIEDSERVO_H +#define MODIFIEDSERVO_H + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +class ModifiedServo : public RTC::DataFlowComponentBase +{ + public: + ModifiedServo(RTC::Manager* manager); + ~ModifiedServo(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + TimedDoubleSeq m_tauRef; + InPort m_tauRefIn; + TimedDoubleSeq m_qRef; + InPort m_qRefIn; + TimedDoubleSeq m_q; + InPort m_qIn; + TimedBooleanSeq m_torqueMode; + InPort m_torqueModeIn; + + // + + // DataOutPort declaration + // + TimedDoubleSeq m_tau; + OutPort m_tauOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + + void readGainFile(); + + hrp::BodyPtr m_robot; + + double dt; // sampling time of the controller + double ref_dt; // sampling time of reference angles + double step; // current interpolation step + double nstep; // number of steps to interpolate references + + size_t dof; + + std::string gain_fname; + std::ifstream gain; + + hrp::dvector Pgain, Dgain; + hrp::dvector q_old, qRef_old; +}; + + +extern "C" +{ + DLL_EXPORT void ModifiedServoInit(RTC::Manager* manager); +}; + +#endif // MODIFIEDSERVO_H + diff --git a/rtc/ModifiedServo/ModifiedServoComp.cpp b/rtc/ModifiedServo/ModifiedServoComp.cpp new file mode 100644 index 00000000000..7ae2be0a511 --- /dev/null +++ b/rtc/ModifiedServo/ModifiedServoComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file ModifiedServoComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ +#include +#include +#include +#include "ModifiedServo.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + ModifiedServoInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("ModifiedServo"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + // RTC::RTObject_var rtobj; + // rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + // PortServiceList* portlist; + // portlist = rtobj->get_ports(); + + // getting port profiles + // std::cout << "Number of Ports: "; + // std::cout << portlist->length() << std::endl << std::endl; + // for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + // { + // Port_ptr port; + // port = (*portlist)[i]; + // std::cout << "Port" << i << " (name): "; + // std::cout << port->get_port_profile()->name << std::endl; + // + // RTC::PortInterfaceProfileList iflist; + // iflist = port->get_port_profile()->interfaces; + // std::cout << "---interfaces---" << std::endl; + // for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + // { + // std::cout << "I/F name: "; + // std::cout << iflist[i].instance_name << std::endl; + // std::cout << "I/F type: "; + // std::cout << iflist[i].type_name << std::endl; + // const char* pol; + // pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + // std::cout << "Polarity: " << pol << std::endl; + // } + // std::cout << "---properties---" << std::endl; + // NVUtil::dump(port->get_port_profile()->properties); + // std::cout << "----------------" << std::endl << std::endl; + // } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} + diff --git a/rtc/NullComponent/CMakeLists.txt b/rtc/NullComponent/CMakeLists.txt index d6240a5ba8f..a800f43a927 100644 --- a/rtc/NullComponent/CMakeLists.txt +++ b/rtc/NullComponent/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(NullComponentComp ${libs}) set(target NullComponent NullComponentComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/NullComponent/NullComponent.cpp b/rtc/NullComponent/NullComponent.cpp index ae5ff0ffd83..10de5362c6b 100644 --- a/rtc/NullComponent/NullComponent.cpp +++ b/rtc/NullComponent/NullComponent.cpp @@ -7,8 +7,8 @@ * $Id$ */ +#include "hrpsys/util/VectorConvert.h" #include "NullComponent.h" -#include "util/VectorConvert.h" // Module specification // diff --git a/rtc/NullComponent/NullComponent.h b/rtc/NullComponent/NullComponent.h index f924ed14700..d9997c4e1d4 100644 --- a/rtc/NullComponent/NullComponent.h +++ b/rtc/NullComponent/NullComponent.h @@ -10,6 +10,7 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H +#include #include #include #include diff --git a/rtc/NullComponent/NullService_impl.h b/rtc/NullComponent/NullService_impl.h index 88bda957a2d..550532b6537 100644 --- a/rtc/NullComponent/NullService_impl.h +++ b/rtc/NullComponent/NullService_impl.h @@ -2,7 +2,7 @@ #ifndef __NULL_SERVICE_H__ #define __NULL_SERVICE_H__ -#include "NullService.hh" +#include "hrpsys/idl/NullService.hh" class NullService_impl : public virtual POA_OpenHRP::NullService, diff --git a/rtc/OGMap3DViewer/CMakeLists.txt b/rtc/OGMap3DViewer/CMakeLists.txt index f6b9e4df567..e8e151b7a94 100644 --- a/rtc/OGMap3DViewer/CMakeLists.txt +++ b/rtc/OGMap3DViewer/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(OGMap3DViewerComp ${libs}) set(target OGMap3DViewer OGMap3DViewerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/OGMap3DViewer/OGMap3DViewer.h b/rtc/OGMap3DViewer/OGMap3DViewer.h index b42bbf59295..922efa5c722 100644 --- a/rtc/OGMap3DViewer/OGMap3DViewer.h +++ b/rtc/OGMap3DViewer/OGMap3DViewer.h @@ -10,6 +10,8 @@ #ifndef OGMAP3DVIEWER_H #define OGMAP3DVIEWER_H +#include +#include #include #include #include @@ -20,7 +22,7 @@ //Open CV headder #include #include -#include "OGMap3DService.hh" +#include "hrpsys/idl/OGMap3DService.hh" class GLbody; class CMapSceneNode; diff --git a/rtc/ObjectContactTurnaroundDetector/CMakeLists.txt b/rtc/ObjectContactTurnaroundDetector/CMakeLists.txt new file mode 100644 index 00000000000..c8dbbca723b --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/CMakeLists.txt @@ -0,0 +1,27 @@ +set(comp_sources ObjectContactTurnaroundDetector.cpp ObjectContactTurnaroundDetectorService_impl.cpp ObjectContactTurnaroundDetectorBase.h ../ImpedanceController/RatsMatrix.cpp ../TorqueFilter/IIRFilter.cpp) +set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +add_library(ObjectContactTurnaroundDetector SHARED ${comp_sources}) +target_link_libraries(ObjectContactTurnaroundDetector ${libs}) +set_target_properties(ObjectContactTurnaroundDetector PROPERTIES PREFIX "") + +add_executable(ObjectContactTurnaroundDetectorComp ObjectContactTurnaroundDetectorComp.cpp ${comp_sources}) +target_link_libraries(ObjectContactTurnaroundDetectorComp ${libs}) +add_executable(testObjectContactTurnaroundDetectorBase testObjectContactTurnaroundDetectorBase.cpp ObjectContactTurnaroundDetectorBase.h ../TorqueFilter/IIRFilter.cpp) +target_link_libraries(testObjectContactTurnaroundDetectorBase ${libs}) + +set(target ObjectContactTurnaroundDetector ObjectContactTurnaroundDetectorComp testObjectContactTurnaroundDetectorBase) + +add_test(testObjectContactTurnaroundDetectorBase0 testObjectContactTurnaroundDetectorBase --test0 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase1 testObjectContactTurnaroundDetectorBase --test1 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase2 testObjectContactTurnaroundDetectorBase --test2 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase3 testObjectContactTurnaroundDetectorBase --test3 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase4 testObjectContactTurnaroundDetectorBase --test4 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase5 testObjectContactTurnaroundDetectorBase --test5 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase6 testObjectContactTurnaroundDetectorBase --test6 --use-gnuplot false) +add_test(testObjectContactTurnaroundDetectorBase7 testObjectContactTurnaroundDetectorBase --test7 --use-gnuplot false) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) + diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.cpp b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.cpp new file mode 100644 index 00000000000..31cffe453ee --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.cpp @@ -0,0 +1,677 @@ +// -*- C++ -*- +/*! + * @file ObjectContactTurnaroundDetector.cpp + * @brief object contact turnaround detector component + * $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include +#include "ObjectContactTurnaroundDetector.h" +#include "../ImpedanceController/RatsMatrix.h" +#include "hrpsys/util/Hrpsys.h" +#include + +typedef coil::Guard Guard; + +// Module specification +// +static const char* objectcontactturnarounddetector_spec[] = + { + "implementation_id", "ObjectContactTurnaroundDetector", + "type_name", "ObjectContactTurnaroundDetector", + "description", "object contact turnaround detector component", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.debugLevel", "0", + "" + }; +// + +ObjectContactTurnaroundDetector::ObjectContactTurnaroundDetector(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_qCurrentIn("qCurrent", m_qCurrent), + m_rpyIn("rpy", m_rpy), + m_contactStatesIn("contactStates", m_contactStates), + m_octdDataOut("octdData", m_octdData), + m_ObjectContactTurnaroundDetectorServicePort("ObjectContactTurnaroundDetectorService"), + // + m_robot(hrp::BodyPtr()), + m_debugLevel(0), + dummy(0) +{ + m_service0.octd(this); +} + +ObjectContactTurnaroundDetector::~ObjectContactTurnaroundDetector() +{ +} + + +RTC::ReturnCode_t ObjectContactTurnaroundDetector::onInitialize() +{ + std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl; + bindParameter("debugLevel", m_debugLevel, "0"); + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("qCurrent", m_qCurrentIn); + addInPort("rpy", m_rpyIn); + addInPort("contactStates", m_contactStatesIn); + + // Set OutPort buffer + addOutPort("octdData", m_octdDataOut); + + // Set service provider to Ports + m_ObjectContactTurnaroundDetectorServicePort.registerProvider("service0", "ObjectContactTurnaroundDetectorService", m_service0); + + // Set service consumers to Ports + + // Set CORBA Service Ports + addPort(m_ObjectContactTurnaroundDetectorServicePort); + + // + // + // Bind variables and configuration variable + + // + + RTC::Properties& prop = getProperties(); + coil::stringTo(m_dt, prop["dt"].c_str()); + + m_robot = hrp::BodyPtr(new hrp::Body()); + + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0){ + comPos = nameServer.length(); + } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), + CosNaming::NamingContext::_duplicate(naming.getRootContext()) + )){ + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl; + return RTC::RTC_ERROR; + } + + + // Setting for wrench data ports (real) + std::vector fsensor_names; + // find names for real force sensors + unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE); + for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); + } + // add ports for all force sensors + unsigned int nforce = npforce; + m_force.resize(nforce); + m_forceIn.resize(nforce); + std::cerr << "[" << m_profile.instance_name << "] force sensor ports" << std::endl; + for (unsigned int i=0; i(fsensor_names[i].c_str(), m_force[i]); + m_force[i].data.length(6); + registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; + } + + unsigned int dof = m_robot->numJoints(); + for ( unsigned int i = 0 ; i < dof; i++ ){ + if ( (int)i != m_robot->joint(i)->jointId ) { + std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl; + return RTC::RTC_ERROR; + } + } + + // setting from conf file + // rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle) + coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ","); + std::map base_name_map; + if (end_effectors_str.size() > 0) { + size_t prop_num = 10; + size_t num = end_effectors_str.size()/prop_num; + for (size_t i = 0; i < num; i++) { + std::string ee_name, ee_target, ee_base; + coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str()); + coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str()); + coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str()); + ee_trans eet; + for (size_t j = 0; j < 3; j++) { + coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); + } + double tmpv[4]; + for (int j = 0; j < 4; j++ ) { + coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str()); + } + eet.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle + eet.target_name = ee_target; + eet.index = i; + ee_map.insert(std::pair(ee_name , eet)); + base_name_map.insert(std::pair(ee_name, ee_base)); + std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; + std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; + } + m_contactStates.data.length(num); + } + + // initialize sensor_names + std::cerr << "[" << m_profile.instance_name << "] Add sensor_names" << std::endl; + for (unsigned int i=0; iname(); + hrp::ForceSensor* sensor = m_robot->sensor(sensor_name); + std::string sensor_link_name; + if ( sensor ) { + // real force sensor + sensor_link_name = sensor->link->name; + } else { + std::cerr << "[" << m_profile.instance_name << "] unknown force param" << std::endl; + continue; + } + // 1. Check whether adequate ee_map exists for the sensor. + std::string ee_name; + bool is_ee_exists = false; + for ( std::map::iterator it = ee_map.begin(); it != ee_map.end(); it++ ) { + hrp::Link* alink = m_robot->link(it->second.target_name); + std::string tmp_base_name = base_name_map[it->first]; + while (alink != NULL && alink->name != tmp_base_name && !is_ee_exists) { + if ( alink->name == sensor_link_name ) { + is_ee_exists = true; + ee_name = it->first; + } + alink = alink->parent; + } + } + if (!is_ee_exists) { + std::cerr << "[" << m_profile.instance_name << "] No such ee setting for " << sensor_name << " and " << sensor_link_name << "!!. sensor_name for " << sensor_name << " cannot be added!!" << std::endl; + continue; + } + // 4. Set impedance param + ee_map[ee_name].sensor_name = sensor_name; + std::cerr << "[" << m_profile.instance_name << "] sensor = " << sensor_name << ", sensor-link = " << sensor_link_name << ", ee_name = " << ee_name << ", ee-link = " << ee_map[ee_name].target_name << std::endl; + } + + octd = boost::shared_ptr(new ObjectContactTurnaroundDetectorBase(m_dt)); + octd->setPrintStr(std::string(m_profile.instance_name)); + + // allocate memory for outPorts + loop = 0; + m_octdData.data.length(4); // mode, raw, filtered, dfiltered + + return RTC::RTC_OK; +} + + + +RTC::ReturnCode_t ObjectContactTurnaroundDetector::onFinalize() +{ + return RTC::RTC_OK; +} + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onStartup(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onShutdown(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +RTC::ReturnCode_t ObjectContactTurnaroundDetector::onActivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ObjectContactTurnaroundDetector::onDeactivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 ) +RTC::ReturnCode_t ObjectContactTurnaroundDetector::onExecute(RTC::UniqueId ec_id) +{ + //std::cout << "ObjectContactTurnaroundDetector::onExecute(" << ec_id << ")" << std::endl; + loop ++; + + // check dataport input + for (unsigned int i=0; iisNew() ) { + m_forceIn[i]->read(); + } + } + if (m_rpyIn.isNew()) { + m_rpyIn.read(); + } + if (m_qCurrentIn.isNew()) { + m_qCurrentIn.read(); + m_octdData.tm = m_qCurrent.tm; + } + if (m_contactStatesIn.isNew()) { + m_contactStatesIn.read(); + } + bool is_contact = false; + for (size_t i = 0; i < m_contactStates.data.length(); i++) { + if (m_contactStates.data[i]) is_contact = true; + } + if ( m_qCurrent.data.length() == m_robot->numJoints() && + is_contact && // one or more limbs are in contact + ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end() ) { // if legged robot + Guard guard(m_mutex); + calcObjectContactTurnaroundDetectorState(); + m_octdDataOut.write(); + } + return RTC::RTC_OK; +} + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onAborting(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onError(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onReset(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onStateUpdate(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +/* + RTC::ReturnCode_t ObjectContactTurnaroundDetector::onRateChanged(RTC::UniqueId ec_id) + { + return RTC::RTC_OK; + } +*/ + +void ObjectContactTurnaroundDetector::calcFootMidCoords (hrp::Vector3& new_foot_mid_pos, hrp::Matrix33& new_foot_mid_rot) +{ + std::vector foot_pos; + std::vector foot_rot; + std::vector leg_names = boost::assign::list_of("rleg")("lleg"); + for (size_t i = 0; i < leg_names.size(); i++) { + hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); + foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos); + foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); + } + new_foot_mid_pos = (foot_pos[0]+foot_pos[1])/2.0; + rats::mid_rot(new_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); +} + +void ObjectContactTurnaroundDetector::calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot) +{ + std::vector leg_c_v; + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 ex = hrp::Vector3::UnitX(); + std::vector leg_names; + for ( std::map::iterator it = ee_map.begin(); it != ee_map.end(); it++ ) { + // If rleg or lleg, and if reference contact states is true + if (it->first.find("leg") != std::string::npos && m_contactStates.data[it->second.index]) leg_names.push_back(it->first); + } + for (size_t i = 0; i < leg_names.size(); i++) { + hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); + rats::coordinates leg_c(hrp::Vector3(target_link->p + target_link->R * ee_map[leg_names[i]].localPos), hrp::Matrix33(target_link->R * ee_map[leg_names[i]].localR)); + hrp::Vector3 xv1(leg_c.rot * ex); + xv1(2)=0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + leg_c.rot(0,0) = xv1(0); leg_c.rot(1,0) = xv1(1); leg_c.rot(2,0) = xv1(2); + leg_c.rot(0,1) = yv1(0); leg_c.rot(1,1) = yv1(1); leg_c.rot(2,1) = yv1(2); + leg_c.rot(0,2) = ez(0); leg_c.rot(1,2) = ez(1); leg_c.rot(2,2) = ez(2); + leg_c_v.push_back(leg_c); + } + if (leg_names.size() == 2) { + rats::coordinates tmpc; + rats::mid_coords(tmpc, 0.5, leg_c_v[0], leg_c_v[1]); + foot_origin_pos = tmpc.pos; + foot_origin_rot = tmpc.rot; + } else { // size = 1 + foot_origin_pos = leg_c_v[0].pos; + foot_origin_rot = leg_c_v[0].rot; + } +} + +void ObjectContactTurnaroundDetector::calcObjectContactTurnaroundDetectorState() +{ + // TODO + // Currently only for legged robots + // Set actual state + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ) { + m_robot->joint(i)->q = m_qCurrent.data[i]; + } + updateRootLinkPosRot(m_rpy); + m_robot->calcForwardKinematics(); + // Calc + std::vector octd_forces, octd_moments, octd_hposv; + hrp::Vector3 fmpos; + hrp::Matrix33 fmrot, fmrotT; + //calcFootMidCoords(fmpos, fmrot); + calcFootOriginCoords(fmpos, fmrot); + fmrotT = fmrot.transpose(); + for (unsigned int i=0; iname(); + if ( find(octd_sensor_names.begin(), octd_sensor_names.end(), sensor_name) != octd_sensor_names.end() ) { + hrp::Vector3 ee_pos; // End Effector Position + for ( std::map::iterator it = ee_map.begin(); it != ee_map.end(); it++ ) { + if ( it->second.sensor_name == sensor_name ) { + ee_trans& eet = it->second; + hrp::Link* target_link = m_robot->link(eet.target_name); + ee_pos = target_link->p + target_link->R * eet.localPos; + } + } + hrp::ForceSensor* sensor = m_robot->sensor(sensor_name); + hrp::Matrix33 sensor_rot = sensor->link->R * sensor->localR; + hrp::Vector3 sensor_pos(sensor->link->R * sensor->localPos + sensor->link->p); + hrp::Vector3 sensor_force(sensor_rot*hrp::Vector3(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2])); + hrp::Vector3 sensor_moment(sensor_rot*hrp::Vector3(m_force[i].data[3], m_force[i].data[4], m_force[i].data[5])); + hrp::Vector3 ee_moment( (sensor_pos - ee_pos).cross(sensor_force) + sensor_moment); + // Change to FootOriginCoords relative values + octd_hposv.push_back(fmrotT*(ee_pos - fmpos)); // Change to FootOriginCoords relative hand pos + octd_forces.push_back(fmrotT*(sensor_force)); // Change to FootOriginCoords relative ee force, and sensor force = ee force + octd_moments.push_back(fmrotT*(ee_moment)); // Change to FootOriginCoords relative ee force + } + } + octd->checkDetection(octd_forces, octd_moments, octd_hposv); + // octdData + hrp::dvector log_data = octd->getDataForLogger(); + if (m_octdData.data.length() != log_data.size()) m_octdData.data.length(log_data.size()); + for (size_t i = 0; i < log_data.size(); i++) { + m_octdData.data[i] = log_data(i); + } +}; + +// + +void ObjectContactTurnaroundDetector::updateRootLinkPosRot (TimedOrientation3D tmprpy) +{ + if ( m_robot->numSensors(hrp::Sensor::ACCELERATION) > 0) { + hrp::Sensor *sensor = m_robot->sensor(hrp::Sensor::ACCELERATION, 0); + hrp::Matrix33 tmpr; + rats::rotm3times(tmpr, hrp::Matrix33(sensor->link->R*sensor->localR).transpose(), m_robot->rootLink()->R); + rats::rotm3times(m_robot->rootLink()->R, hrp::rotFromRpy(tmprpy.data.r, tmprpy.data.p, tmprpy.data.y), tmpr); + m_robot->rootLink()->p = hrp::Vector3::Zero(); + } +} + +// +// ObjectContactTurnaroundDetector +// + +void ObjectContactTurnaroundDetector::startObjectContactTurnaroundDetection(const double i_ref_diff_wrench, const double i_max_time, const OpenHRP::ObjectContactTurnaroundDetectorService::StrSequence& i_ee_names) +{ + Guard guard(m_mutex); + octd->startDetection(i_ref_diff_wrench, i_max_time); + octd_sensor_names.clear(); + for (size_t i = 0; i < i_ee_names.length(); i++) { + octd_sensor_names.push_back(ee_map[std::string(i_ee_names[i])].sensor_name); + } +} + +bool ObjectContactTurnaroundDetector::startObjectContactTurnaroundDetectionForGeneralizedWrench () +{ + Guard guard(m_mutex); + octd->startDetectionForGeneralizedWrench(); + return true; +} + +OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode ObjectContactTurnaroundDetector::checkObjectContactTurnaroundDetectionCommon(const size_t index) +{ + OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode tmpmode; + switch (octd->getMode(index)) { + case ObjectContactTurnaroundDetectorBase::MODE_IDLE: + tmpmode = ObjectContactTurnaroundDetectorService::MODE_DETECTOR_IDLE; + break; + case ObjectContactTurnaroundDetectorBase::MODE_STARTED: + tmpmode = ObjectContactTurnaroundDetectorService::MODE_STARTED; + break; + case ObjectContactTurnaroundDetectorBase::MODE_DETECTED: + tmpmode = ObjectContactTurnaroundDetectorService::MODE_DETECTED; + break; + case ObjectContactTurnaroundDetectorBase::MODE_MAX_TIME: + tmpmode = ObjectContactTurnaroundDetectorService::MODE_MAX_TIME; + break; + case ObjectContactTurnaroundDetectorBase::MODE_OTHER_DETECTED: + tmpmode = ObjectContactTurnaroundDetectorService::MODE_OTHER_DETECTED; + break; + default: + tmpmode = ObjectContactTurnaroundDetectorService::MODE_DETECTOR_IDLE; + break; + } + return tmpmode; +} + +OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode ObjectContactTurnaroundDetector::checkObjectContactTurnaroundDetection() +{ + Guard guard(m_mutex); + return checkObjectContactTurnaroundDetectionCommon(0); +} + +bool ObjectContactTurnaroundDetector::checkObjectContactTurnaroundDetectionForGeneralizedWrench(OpenHRP::ObjectContactTurnaroundDetectorService::DetectorModeSequence_out o_dms) +{ + Guard guard(m_mutex); + o_dms->length(octd->getDetectGeneralizedWrenchDim()); + for (size_t i = 0; i < octd->getDetectGeneralizedWrenchDim(); i++) { + o_dms[i] = checkObjectContactTurnaroundDetectionCommon(i); + } + return true; +} + +bool ObjectContactTurnaroundDetector::setObjectContactTurnaroundDetectorParam(const OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam &i_param_) +{ + Guard guard(m_mutex); + std::cerr << "[" << m_profile.instance_name << "] setObjectContactTurnaroundDetectorParam" << std::endl; + octd->setWrenchCutoffFreq(i_param_.wrench_cutoff_freq); + octd->setDwrenchCutoffFreq(i_param_.dwrench_cutoff_freq); + octd->setDetectRatioThre(i_param_.detect_ratio_thre); + octd->setStartRatioThre(i_param_.start_ratio_thre); + octd->setDetectTimeThre(i_param_.detect_time_thre); + octd->setStartTimeThre(i_param_.start_time_thre); + octd->setOtherDetectTimeThre(i_param_.other_detect_time_thre); + octd->setForgettingRatioThre(i_param_.forgetting_ratio_thre); + hrp::Vector3 tmp; + for (size_t i = 0; i < 3; i++) tmp(i) = i_param_.axis[i]; + octd->setAxis(tmp); + for (size_t i = 0; i < 3; i++) tmp(i) = i_param_.moment_center[i]; + octd->setMomentCenter(tmp); + ObjectContactTurnaroundDetectorBase::detector_total_wrench dtw; + switch (i_param_.detector_total_wrench) { + case OpenHRP::ObjectContactTurnaroundDetectorService::TOTAL_FORCE: + dtw = ObjectContactTurnaroundDetectorBase::TOTAL_FORCE; + break; + case OpenHRP::ObjectContactTurnaroundDetectorService::TOTAL_MOMENT: + dtw = ObjectContactTurnaroundDetectorBase::TOTAL_MOMENT; + break; + case OpenHRP::ObjectContactTurnaroundDetectorService::TOTAL_MOMENT2: + dtw = ObjectContactTurnaroundDetectorBase::TOTAL_MOMENT2; + break; + case OpenHRP::ObjectContactTurnaroundDetectorService::GENERALIZED_WRENCH: + dtw = ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH; + break; + default: + break; + } + octd->setDetectorTotalWrench(dtw); + octd->setIsHoldValues(i_param_.is_hold_values); + // For GENERALIZED_WRENCH mode + if ( (i_param_.constraint_conversion_matrix1.length() % 6 == 0) && + (i_param_.constraint_conversion_matrix1.length() == i_param_.constraint_conversion_matrix2.length()) && + (i_param_.constraint_conversion_matrix1.length() == i_param_.ref_dphi1.length()*6) ) { + size_t row_dim = i_param_.constraint_conversion_matrix1.length()/6; + std::vector tmpccm1(row_dim, hrp::dvector6::Zero()); + for (size_t i = 0; i < row_dim; i++) { + for (size_t j = 0; j < 6; j++) { + tmpccm1[i](j) = i_param_.constraint_conversion_matrix1[i*6+j]; + } + } + std::vector tmpccm2(row_dim, hrp::dvector6::Zero()); + for (size_t i = 0; i < row_dim; i++) { + for (size_t j = 0; j < 6; j++) { + tmpccm2[i](j) = i_param_.constraint_conversion_matrix2[i*6+j]; + } + } + std::vector tmp_ref_dphi1; + for (size_t i = 0; i < i_param_.ref_dphi1.length(); i++) tmp_ref_dphi1.push_back(i_param_.ref_dphi1[i]); + octd->setConstraintConversionMatricesRefDwrench(tmpccm1, tmpccm2, tmp_ref_dphi1); + } else { + std::cerr << "[" << m_profile.instance_name << "] Invalid constraint_conversion_matrix size (ccm1 = " + << i_param_.constraint_conversion_matrix1.length() << ", ccm2 = " << i_param_.constraint_conversion_matrix2.length() << ", ref_dw = " << i_param_.ref_dphi1.length() << ")" << std::endl; + return false; + } + octd->setMaxTime(i_param_.max_time); + octd_sensor_names.clear(); + for (size_t i = 0; i < i_param_.limbs.length(); i++) { + octd_sensor_names.push_back(ee_map[std::string(i_param_.limbs[i])].sensor_name); + } + // Print + octd->printParams(); + std::cerr << "[" << m_profile.instance_name << "] sensor_names = ["; + for (size_t i = 0; i < octd_sensor_names.size(); i++) std::cerr << getEENameFromSensorName(octd_sensor_names[i]) << " "; + std::cerr << "]" << std::endl; + return true; +}; + +bool ObjectContactTurnaroundDetector::getObjectContactTurnaroundDetectorParam(OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam& i_param_) +{ + std::cerr << "[" << m_profile.instance_name << "] getObjectContactTurnaroundDetectorParam" << std::endl; + i_param_.wrench_cutoff_freq = octd->getWrenchCutoffFreq(); + i_param_.dwrench_cutoff_freq = octd->getDwrenchCutoffFreq(); + i_param_.detect_ratio_thre = octd->getDetectRatioThre(); + i_param_.start_ratio_thre = octd->getStartRatioThre(); + i_param_.detect_time_thre = octd->getDetectTimeThre(); + i_param_.start_time_thre = octd->getStartTimeThre(); + i_param_.other_detect_time_thre = octd->getOtherDetectTimeThre(); + i_param_.forgetting_ratio_thre = octd->getForgettingRatioThre(); + hrp::Vector3 tmp = octd->getAxis(); + for (size_t i = 0; i < 3; i++) i_param_.axis[i] = tmp(i); + tmp = octd->getMomentCenter(); + for (size_t i = 0; i < 3; i++) i_param_.moment_center[i] = tmp(i); + OpenHRP::ObjectContactTurnaroundDetectorService::DetectorTotalWrench dtw; + switch (octd->getDetectorTotalWrench()) { + case ObjectContactTurnaroundDetectorBase::TOTAL_FORCE: + dtw = OpenHRP::ObjectContactTurnaroundDetectorService::TOTAL_FORCE; + break; + case ObjectContactTurnaroundDetectorBase::TOTAL_MOMENT: + dtw = OpenHRP::ObjectContactTurnaroundDetectorService::TOTAL_MOMENT; + break; + case ObjectContactTurnaroundDetectorBase::TOTAL_MOMENT2: + dtw = OpenHRP::ObjectContactTurnaroundDetectorService::TOTAL_MOMENT2; + break; + case ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH: + dtw = OpenHRP::ObjectContactTurnaroundDetectorService::GENERALIZED_WRENCH; + break; + default: + break; + } + i_param_.detector_total_wrench = dtw; + i_param_.is_hold_values = octd->getIsHoldValues(); + // For GENERALIZED_WRENCH mode + std::vector tmpccm1, tmpccm2; + std::vector tmp_ref_dphi1; + octd->getConstraintConversionMatricesRefDwrench(tmpccm1, tmpccm2, tmp_ref_dphi1); + i_param_.constraint_conversion_matrix1.length(tmpccm1.size()*6); + for (size_t i = 0; i < tmpccm1.size(); i++) { + for (size_t j = 0; j < 6; j++) { + i_param_.constraint_conversion_matrix1[i*6+j] = tmpccm1[i](j); + } + } + i_param_.constraint_conversion_matrix2.length(tmpccm2.size()*6); + for (size_t i = 0; i < tmpccm2.size(); i++) { + for (size_t j = 0; j < 6; j++) { + i_param_.constraint_conversion_matrix2[i*6+j] = tmpccm2[i](j); + } + } + i_param_.ref_dphi1.length(tmp_ref_dphi1.size()); + for (size_t i = 0; i < tmp_ref_dphi1.size(); i++) i_param_.ref_dphi1[i] = tmp_ref_dphi1[i]; + i_param_.max_time = octd->getMaxTime(); + i_param_.limbs.length(octd_sensor_names.size()); + for (size_t i = 0; i < octd_sensor_names.size(); i++) { + i_param_.limbs[i] = getEENameFromSensorName(octd_sensor_names[i]).c_str(); + } + return true; +} + +bool ObjectContactTurnaroundDetector::getObjectForcesMoments(OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_forces, OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_moments, OpenHRP::ObjectContactTurnaroundDetectorService::DblSequence3_out o_3dofwrench, double& o_fric_coeff_wrench) +{ + std::cerr << "[" << m_profile.instance_name << "] getObjectForcesMoments" << std::endl; + if (octd_sensor_names.size() == 0) return false; + hrp::Vector3 tmpv = octd->getAxis() * octd->getFilteredWrenchWithHold()[0]; + o_forces = new OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence (); + o_moments = new OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence (); + o_forces->length(octd_sensor_names.size()); + o_moments->length(octd_sensor_names.size()); + for (size_t i = 0; i < o_forces->length(); i++) o_forces[i].length(3); + for (size_t i = 0; i < o_moments->length(); i++) o_moments[i].length(3); + // Temp + for (size_t i = 0; i < octd_sensor_names.size(); i++) { + o_forces[i][0] = tmpv(0)/octd_sensor_names.size(); + o_forces[i][1] = tmpv(1)/octd_sensor_names.size(); + o_forces[i][2] = tmpv(2)/octd_sensor_names.size(); + o_moments[i][0] = o_moments[i][1] = o_moments[i][2] = 0.0; + } + o_3dofwrench = new OpenHRP::ObjectContactTurnaroundDetectorService::DblSequence3 (); + o_3dofwrench->length(3); + for (size_t i = 0; i < 3; i++) (*o_3dofwrench)[i] = tmpv(i); + o_fric_coeff_wrench = octd->getFilteredFrictionCoeffWrenchWithHold()[0]; + return true; +} + +bool ObjectContactTurnaroundDetector::getObjectGeneralizedConstraintWrenches(OpenHRP::ObjectContactTurnaroundDetectorService::objectGeneralizedConstraintWrenchesParam& o_param) +{ + std::vector tmp1 = octd->getFilteredWrenchWithHold(); + o_param.generalized_constraint_wrench1.length(tmp1.size()); + for (size_t i = 0; i < tmp1.size(); i++) o_param.generalized_constraint_wrench1[i] = tmp1[i]; + std::vector tmp2 = octd->getFilteredFrictionCoeffWrenchWithHold(); + o_param.generalized_constraint_wrench2.length(tmp2.size()); + for (size_t i = 0; i < tmp2.size(); i++) o_param.generalized_constraint_wrench2[i] = tmp2[i]; + hrp::dvector6 tmpr = octd->getFilteredResultantWrenchWithHold(); + for (size_t i = 0; i < 6; i++) o_param.resultant_wrench[i] = tmpr(i); + return true; +} + +extern "C" +{ + + void ObjectContactTurnaroundDetectorInit(RTC::Manager* manager) + { + RTC::Properties profile(objectcontactturnarounddetector_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.h b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.h new file mode 100644 index 00000000000..979b22b009a --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.h @@ -0,0 +1,184 @@ +// -*- C++ -*- +/*! + * @file ObjectContactTurnaroundDetector.h + * @brief object contact turnaround detector component + * @date $Date$ + * + * $Id$ + */ + +#ifndef OBJECTCONTACTTURNAROUNDDETECTOR_H +#define OBJECTCONTACTTURNAROUNDDETECTOR_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ObjectContactTurnaroundDetectorBase.h" +// Service implementation headers +// +#include "ObjectContactTurnaroundDetectorService_impl.h" + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +class ObjectContactTurnaroundDetector + : public RTC::DataFlowComponentBase +{ + public: + ObjectContactTurnaroundDetector(RTC::Manager* manager); + virtual ~ObjectContactTurnaroundDetector(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + void startObjectContactTurnaroundDetection(const double i_ref_diff_wrench, const double i_max_time, const OpenHRP::ObjectContactTurnaroundDetectorService::StrSequence& i_ee_names); + OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode checkObjectContactTurnaroundDetection(); + bool setObjectContactTurnaroundDetectorParam(const OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam &i_param_); + bool getObjectContactTurnaroundDetectorParam(OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam& i_param_); + bool getObjectForcesMoments(OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_forces, OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_moments, OpenHRP::ObjectContactTurnaroundDetectorService::DblSequence3_out o_3dofwrench, double& o_fric_coeff_wrench); + bool checkObjectContactTurnaroundDetectionForGeneralizedWrench(OpenHRP::ObjectContactTurnaroundDetectorService::DetectorModeSequence_out o_dms); + bool startObjectContactTurnaroundDetectionForGeneralizedWrench(); + bool getObjectGeneralizedConstraintWrenches(OpenHRP::ObjectContactTurnaroundDetectorService::objectGeneralizedConstraintWrenchesParam& o_param); + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + TimedDoubleSeq m_qCurrent; + InPort m_qCurrentIn; + std::vector m_force; + std::vector *> m_forceIn; + TimedOrientation3D m_rpy; + InPort m_rpyIn; + TimedBooleanSeq m_contactStates; + InPort m_contactStatesIn; + + // + + // DataOutPort declaration + // + TimedDoubleSeq m_octdData; + OutPort m_octdDataOut; + + // + + // CORBA Port declaration + // + RTC::CorbaPort m_ObjectContactTurnaroundDetectorServicePort; + + // + + // Service declaration + // + ObjectContactTurnaroundDetectorService_impl m_service0; + + // + + // Consumer declaration + // + + // + + private: + + struct ee_trans { + std::string target_name, sensor_name; + hrp::Vector3 localPos; + hrp::Matrix33 localR; + size_t index; + }; + + void updateRootLinkPosRot (TimedOrientation3D tmprpy); + void calcFootMidCoords (hrp::Vector3& new_foot_mid_pos, hrp::Matrix33& new_foot_mid_rot); + void calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot); + void calcObjectContactTurnaroundDetectorState(); + OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode checkObjectContactTurnaroundDetectionCommon(const size_t index); + std::string getEENameFromSensorName (const std::string& sensor_name) + { + std::string tmp_ee_name(""); + for (std::map::const_iterator it = ee_map.begin(); it != ee_map.end(); it++) if (it->second.sensor_name == sensor_name) tmp_ee_name = it->first; + return tmp_ee_name; + }; + + std::map ee_map; + boost::shared_ptr octd; + std::vector octd_sensor_names; + hrp::Vector3 octd_axis; + double m_dt; + hrp::BodyPtr m_robot; + coil::Mutex m_mutex; + unsigned int m_debugLevel; + int dummy; + int loop; +}; + + +extern "C" +{ + void ObjectContactTurnaroundDetectorInit(RTC::Manager* manager); +}; + +#endif // OBJECTCONTACTTURNAROUNDDETECTOR_H diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.txt b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.txt new file mode 100644 index 00000000000..0d5a697af82 --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetector.txt @@ -0,0 +1,59 @@ +/** + +\page ObjectContactTurnaroundDetector + +\section introduction Overview + +This component is octd. + +\subsection feature Feature + + + + +
    implementation_idObjectContactTurnaroundDetector
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + + + +
    port namedata typeunitdescription
    qCurrentRTC::TimedDoubleSeq[rad]Actual joint angles
    rpyRTC::TimedOrientation3D[rad] +Actual attitude sensor's Roll-Pitch-Yaw angle
    name of force/torque sensor defined in a VRML model, such as "rhsensor"RTC::TimedDoubleSeq[N],[Nm]Actual force/torque in the sensor frame
    + +\subsection outports Output Ports + + + +
    port namedata typeunitdescription
    + +\section serviceports Service Ports + +\subsection provider Service Providers + + + + +
    port nameinterface nameservice typeIDLdescription
    ObjectContactTurnaroundDetectorServiceservice0ObjectContactTurnaroundDetectorService\ref OpenHRP::ObjectContactTurnaroundDetectorServiceObjectContactTurnaroundDetectorService
    + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + + +
    nametypeunitdefault +valuedescription
    debugLevelint0debug level
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorBase.h b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorBase.h new file mode 100644 index 00000000000..3930d823a0a --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorBase.h @@ -0,0 +1,407 @@ +#ifndef OBJECTCONTACTTURNAROUNDDETECTORBASE_H +#define OBJECTCONTACTTURNAROUNDDETECTORBASE_H + +#include "../TorqueFilter/IIRFilter.h" +#include +#include +#include +#include "hrpsys/util/Hrpsys.h" +#include + +class ObjectContactTurnaroundDetectorBase +{ + public: + typedef enum {MODE_IDLE, MODE_STARTED, MODE_DETECTED, MODE_MAX_TIME, MODE_OTHER_DETECTED} process_mode; + typedef enum {TOTAL_FORCE, TOTAL_MOMENT, TOTAL_MOMENT2, GENERALIZED_WRENCH} detector_total_wrench; + private: + boost::shared_ptr > wrench_filter; + boost::shared_ptr > dwrench_filter; + boost::shared_ptr > friction_coeff_wrench_filter; + boost::shared_ptr > resultant_wrench_filter; + boost::shared_ptr > resultant_dwrench_filter; + hrp::Vector3 axis, moment_center; + std::vector constraint_conversion_matrix1, constraint_conversion_matrix2; + hrp::dvector6 filtered_resultant_wrench_with_hold; + double dt; + double detect_ratio_thre, start_ratio_thre, forgetting_ratio_thre, max_time, current_time; + // detect_count_thre*dt, start_ratio_thre*dt, and other_detect_count_thre*dt are threshould for time. + // detect_count_thre*dt : Threshould for time [s] after the first object contact turnaround detection (Wait detect_time_thre [s] after first object contact turnaround detection). + // start_count_thre*dt : Threshould for time [s] after the first starting detection (Wait start_time_thre [s] after first start detection). + // other_detect_count_thre*dt : Threshould for time [s] to move to MODE_OTHER_DETECTED after the first MODE_DETECTED, that is, do not check contact change other than detected element. + size_t detect_count_thre, start_count_thre, other_detect_count_thre; + detector_total_wrench dtw; + std::string print_str; + bool is_filter_reset, is_hold_values, is_other_constraint_detected; + // Parameters which size can be changed, especially for GENERALIZED_WRENCH mode + std::vector ref_dwrench, raw_wrench, filtered_wrench_with_hold, filtered_friction_coeff_wrench_with_hold, phi1, phi2, dphi1; + std::vector pmode; + std::vector count; + public: + ObjectContactTurnaroundDetectorBase (const double _dt) : axis(-1*hrp::Vector3::UnitZ()), moment_center(hrp::Vector3::Zero()), + constraint_conversion_matrix1(std::vector(1, hrp::dvector6::Zero())), constraint_conversion_matrix2(std::vector(1, hrp::dvector6::Zero())), filtered_resultant_wrench_with_hold(hrp::dvector6::Zero()), + dt(_dt), detect_ratio_thre(0.01), start_ratio_thre(0.5), forgetting_ratio_thre(1e3), // Too large threshold for forgetting ratio. Forgetting ratio is disabled by default. + max_time(0.0), current_time(0.0), + detect_count_thre(5), start_count_thre(5), other_detect_count_thre(round(detect_count_thre*1.5)), dtw(TOTAL_FORCE), + is_filter_reset(false), is_hold_values(false), is_other_constraint_detected(false), + ref_dwrench(std::vector(1, 0.0)), raw_wrench(std::vector(1, 0.0)), filtered_wrench_with_hold(std::vector(1, 0.0)), filtered_friction_coeff_wrench_with_hold(std::vector(1, 0.0)), + phi1(std::vector(1, 0.0)), phi2(std::vector(1, 0.0)), dphi1(std::vector(1, 0.0)), + pmode(std::vector(1, MODE_MAX_TIME)), count(std::vector(1)) + + { + double default_cutoff_freq = 1; // [Hz] + wrench_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(default_cutoff_freq, _dt, 0)); + dwrench_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(default_cutoff_freq, _dt, 0)); + friction_coeff_wrench_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(default_cutoff_freq, _dt, 0)); + resultant_wrench_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(default_cutoff_freq, _dt, hrp::dvector6::Zero())); + resultant_dwrench_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(default_cutoff_freq, _dt, hrp::dvector6::Zero())); + }; + ~ObjectContactTurnaroundDetectorBase () {}; + void startDetection (const double _ref_diff_wrench, const double _max_time) + { + if (ref_dwrench.size() != 1) ref_dwrench.resize(1); + resizeVariablesForGeneralizedWrench(1); + // NOTE : _ref_diff_wrench is difference. d_xx is velocity. + ref_dwrench[0] = _ref_diff_wrench/_max_time; + max_time = _max_time; + startDetectionForGeneralizedWrench(); + }; + void startDetectionForGeneralizedWrench () + { + for (size_t i = 0; i < count.size(); i++) { + count[i] = 0; + pmode[i] = MODE_IDLE; + } + current_time = 0; + is_filter_reset = true; + is_other_constraint_detected = false; + std::cerr << "[" << print_str << "] Start Object Turnaround Detection ("; + std::cerr << "ref_dwrench = ["; + for (size_t i = 0; i < ref_dwrench.size(); i++) std::cerr << ref_dwrench[i] << ", "; + std::cerr << "], detect_thre = ["; + for (size_t i = 0; i < ref_dwrench.size(); i++) std::cerr << ref_dwrench[i] * detect_ratio_thre << ", "; + std::cerr << "], start_thre = ["; + for (size_t i = 0; i < ref_dwrench.size(); i++) std::cerr << ref_dwrench[i] * start_ratio_thre << ", "; + std::cerr << "]), max_time = " << max_time << "[s]" << std::endl; + }; + void resizeVariablesForGeneralizedWrench (size_t generalized_wrench_dim) + { + phi1.resize(generalized_wrench_dim); + phi2.resize(generalized_wrench_dim); + dphi1.resize(generalized_wrench_dim); + filtered_wrench_with_hold.resize(generalized_wrench_dim); + filtered_friction_coeff_wrench_with_hold.resize(generalized_wrench_dim); + raw_wrench.resize(generalized_wrench_dim); + count.resize(generalized_wrench_dim); + pmode.resize(generalized_wrench_dim); + for (size_t i = 0; i < generalized_wrench_dim; i++) { + count[i] = 0; + pmode[i] = MODE_MAX_TIME; + } + }; + void calcTotalForceMoment (hrp::Vector3& total_force, hrp::Vector3& total_moment1, hrp::Vector3& total_moment2, + const std::vector& forces, const std::vector& moments, const std::vector& hposv) + { + // Total wrench around the origin + total_force = total_moment1 = total_moment2 = hrp::Vector3::Zero(); + for (size_t i = 0; i < forces.size(); i++) { + total_force += forces[i]; + total_moment1 += hposv[i].cross(forces[i]); + total_moment2 += moments[i]; + } + }; + bool checkDetection (const std::vector& forces, + const std::vector& moments, + const std::vector& hposv) + { + // Calculate total force and moments + // forces, moments, hposv : Force, moment, position for all EE : f_i, n_i, p_i + // total_force : F = \sum_i f_i + // total_moment1 : M1 = \sum_i p_i \times f_i = \sum_i p_i \times f_i - p_c \times F + // total_moment2 : M2 = \sum_i n_i + hrp::Vector3 total_force, total_moment1, total_moment2; + calcTotalForceMoment(total_force, total_moment1, total_moment2, forces, moments, hposv); + // Calculate generalized force/moment values and check detection + bool ret = false; + switch(dtw) { + case TOTAL_FORCE: + { + ret = checkDetection(axis.dot(total_force), total_force(2)); + break; + } + case TOTAL_MOMENT: + { + // \sum_i (p_i - p_c) \times f_i = M1 - p_c \times F + ret = checkDetection(axis.dot(total_moment1 - moment_center.cross(total_force)), 0.0); + } + break; + case TOTAL_MOMENT2: + { + // \sum_i (p_i - p_c) \times f_i + n_i = M1 + M2 - p_c \times F + ret = checkDetection(axis.dot(total_moment1 + total_moment2 - moment_center.cross(total_force)), 0.0); + } + break; + case GENERALIZED_WRENCH: + { + hrp::dvector6 resultant_OR_wrench; + for (size_t i = 0; i < 3; i++) { + resultant_OR_wrench(i) = total_force(i); + resultant_OR_wrench(i+3) = total_moment1(i) + total_moment2(i); + } + ret = checkDetection(resultant_OR_wrench); + }; + break; + default: + break; + }; + return ret; + }; + bool checkDetection (const double raw_wrench_value, const double raw_friction_coeff_wrench_value) + { + if (is_filter_reset) { + std::cerr << "[" << print_str << "] Object Turnaround Detection Reset Values. (raw_wrench_value = " << raw_wrench_value << ", raw_friction_coeff_wrench_value = " << raw_friction_coeff_wrench_value << ")" << std::endl; + wrench_filter->reset(raw_wrench_value); + dwrench_filter->reset(0); + friction_coeff_wrench_filter->reset(raw_friction_coeff_wrench_value); + filtered_wrench_with_hold[0] = wrench_filter->getCurrentValue(); + filtered_friction_coeff_wrench_with_hold[0] = friction_coeff_wrench_filter->getCurrentValue(); + is_filter_reset = false; + } + raw_wrench[0] = raw_wrench_value; + double prev_filtered_wrench = wrench_filter->getCurrentValue(); + double filtered_wrench = wrench_filter->passFilter(raw_wrench_value); + double filtered_dwrench = dwrench_filter->passFilter((filtered_wrench-prev_filtered_wrench)/dt); + friction_coeff_wrench_filter->passFilter(raw_friction_coeff_wrench_value); + // Hold values : is_hold_values is true and previously "detected", hold values. Otherwise, update values. + if ( !(is_hold_values && isDetected()) ) { + filtered_wrench_with_hold[0] = wrench_filter->getCurrentValue(); + filtered_friction_coeff_wrench_with_hold[0] = friction_coeff_wrench_filter->getCurrentValue(); + } + // For logging + phi1[0] = wrench_filter->getCurrentValue(); + phi2[0] = friction_coeff_wrench_filter->getCurrentValue(); + dphi1[0] = dwrench_filter->getCurrentValue(); + return updateProcessModeFromDwrench(std::vector(1, filtered_dwrench)); + }; + bool checkDetection (const hrp::dvector6& raw_resultant_wrench_value) + { + if (is_filter_reset) { + std::cerr << "[" << print_str << "] Object Turnaround Detection Reset Values. (raw_resultant_wrench_value = " << raw_resultant_wrench_value.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << ")" << std::endl; + resultant_wrench_filter->reset(raw_resultant_wrench_value); + resultant_dwrench_filter->reset(hrp::dvector6::Zero()); + } + hrp::dvector6 prev_filtered_resultant_wrench = resultant_wrench_filter->getCurrentValue(); + hrp::dvector6 filtered_resultant_wrench = resultant_wrench_filter->passFilter(raw_resultant_wrench_value); + hrp::dvector6 filtered_resultant_dwrench = resultant_dwrench_filter->passFilter((filtered_resultant_wrench - prev_filtered_resultant_wrench)/dt); + calcPhiValueFromConstraintConversionMatrix(phi1, constraint_conversion_matrix1, filtered_resultant_wrench); + calcPhiValueFromConstraintConversionMatrix(phi2, constraint_conversion_matrix2, filtered_resultant_wrench); + calcPhiValueFromConstraintConversionMatrix(dphi1, constraint_conversion_matrix1, filtered_resultant_dwrench); + if (is_filter_reset) { + filtered_wrench_with_hold = phi1; + filtered_friction_coeff_wrench_with_hold = phi2; + filtered_resultant_wrench_with_hold = filtered_resultant_wrench; + is_filter_reset = false; + } + // Hold values : is_hold_values is true and previously "detected", hold values. Otherwise, update values. + if ( !(is_hold_values && isDetected()) ) { + filtered_wrench_with_hold = phi1; + filtered_friction_coeff_wrench_with_hold = phi2; + filtered_resultant_wrench_with_hold = filtered_resultant_wrench; + } + // For logger, just used as buffer + calcPhiValueFromConstraintConversionMatrix(raw_wrench, constraint_conversion_matrix1, raw_resultant_wrench_value); + // Update process mode and return + return updateProcessModeFromDwrench(dphi1); + }; + void calcPhiValueFromConstraintConversionMatrix (std::vector& phi, const std::vector& ccm, const hrp::dvector6& res_wrench) + { + for (size_t i = 0; i < ccm.size(); i++) phi[i] = ccm[i].dot(res_wrench); + }; + bool updateProcessModeFromDwrench (const std::vector& tmp_dwrench) + { + // Checking of wrench profile turn around + // Sign of ref_dwrench and tmp_dwrench shuold be same + // Supprot both ref_dwrench > 0 case and ref_dwrench < 0 case + for (size_t i = 0; i < ref_dwrench.size(); i++) { + if (!is_other_constraint_detected) { + switch (pmode[i]) { + case MODE_IDLE: + if ( (ref_dwrench[i] > 0.0) ? (tmp_dwrench[i] > ref_dwrench[i]*start_ratio_thre) : (tmp_dwrench[i] < ref_dwrench[i]*start_ratio_thre) ) { + count[i]++; + if (count[i] > start_count_thre) { + pmode[i] = MODE_STARTED; + count[i] = 0; + std::cerr << "[" << print_str << "] Object Turnaround Detection Started [idx=" << i << "]. (time = " << current_time << "[s], " << start_count_thre*dt << "[s] after the first start detection)" << std::endl; + } + } else { + /* count--; */ + } + break; + case MODE_STARTED: + if ( (ref_dwrench[i] > 0.0) ? (tmp_dwrench[i] < ref_dwrench[i]*detect_ratio_thre) : (tmp_dwrench[i] > ref_dwrench[i]*detect_ratio_thre) ) { + count[i]++; + if (count[i] > detect_count_thre) { + pmode[i] = MODE_DETECTED; + count[i] = 0; + std::cerr << "[" << print_str << "] Object Turnaround Detected [idx=" << i << "]. (time = " << current_time << "[s], " << detect_count_thre*dt << "[s] after the first detection)" << std::endl; + } + } else { + if ( ((ref_dwrench[i] > 0.0) ? (tmp_dwrench[i] > ref_dwrench[i]*forgetting_ratio_thre) : (tmp_dwrench[i] < ref_dwrench[i]*forgetting_ratio_thre)) && + (count[i] > 0) ) { + count[i]--; + } + } + break; + case MODE_DETECTED: + { + count[i]++; + if (count[i] > other_detect_count_thre) { + is_other_constraint_detected = true; + std::cerr << "[" << print_str << "] Object Turnaround Other Detected Time Limit [idx=" << i << "]. (time = " << current_time << "[s], " << other_detect_count_thre*dt << "[s] after the first detection)" << std::endl; + } + } + break; + case MODE_MAX_TIME: + break; + default: + break; + } + if (max_time <= current_time && (pmode[i] != MODE_DETECTED)) { + if (pmode[i] != MODE_MAX_TIME) std::cerr << "[" << print_str << "] Object Turnaround Detection max time reached. [idx=" << i << "]" << std::endl; + pmode[i] = MODE_MAX_TIME; + } + } else { + if (pmode[i] != MODE_DETECTED) pmode[i] = MODE_OTHER_DETECTED; + } + } + current_time += dt; + return isDetected(); + }; + bool isDetected (const size_t idx) const { return (pmode[idx] == MODE_DETECTED); }; + bool isDetected () const + { + for (size_t i = 0; i < pmode.size(); i++) { + if (isDetected(i)) return true; + } + return false; + }; + void printParams () const + { + std::string tmpstr; + switch (dtw) { + case TOTAL_FORCE: + tmpstr = "TOTAL_FORCE";break; + case TOTAL_MOMENT: + tmpstr = "TOTAL_MOMENT";break; + case TOTAL_MOMENT2: + tmpstr = "TOTAL_MOMENT2";break; + case GENERALIZED_WRENCH: + tmpstr = "GENERALIZED_WRENCH";break; + default: + tmpstr = "";break; + } + std::cerr << "[" << print_str << "] ObjectContactTurnaroundDetectorBase params (" << tmpstr << ")" << std::endl; + std::cerr << "[" << print_str << "] wrench_cutoff_freq = " << wrench_filter->getCutOffFreq() << "[Hz], dwrench_cutoff_freq = " << dwrench_filter->getCutOffFreq() << "[Hz], friction_coeff_wrench_freq = " << friction_coeff_wrench_filter->getCutOffFreq() + << "[Hz], resultant_wrench_cutoff_freq = " << resultant_wrench_filter->getCutOffFreq() << "[Hz], resultant_dwrench_cutoff_freq = " << resultant_dwrench_filter->getCutOffFreq() << "[Hz]" << std::endl; + std::cerr << "[" << print_str << "] detect_ratio_thre = " << detect_ratio_thre << ", start_ratio_thre = " << start_ratio_thre << ", forgetting_ratio_thre = " << forgetting_ratio_thre + << ", start_time_thre = " << start_count_thre*dt << "[s], detect_time_thre = " << detect_count_thre*dt << "[s], other_detect_time_thre = " << other_detect_count_thre*dt << std::endl; + std::cerr << "[" << print_str << "] axis = [" << axis(0) << ", " << axis(1) << ", " << axis(2) + << "], moment_center = " << moment_center(0) << ", " << moment_center(1) << ", " << moment_center(2) << "][m]" << std::endl; + std::cerr << "[" << print_str << "] constraint_conversion_matrix1 = ["; + for (size_t i = 0; i < constraint_conversion_matrix1.size(); i++) { + std::cerr << constraint_conversion_matrix1[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")); + } + std::cerr << "], constraint_conversion_matrix2 = ["; + for (size_t i = 0; i < constraint_conversion_matrix2.size(); i++) { + std::cerr << constraint_conversion_matrix2[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")); + } + std::cerr << "]" << std::endl; + std::cerr << "[" << print_str << "] is_hold_values = " << (is_hold_values?"true":"false") << std::endl; + std::cerr << "[" << print_str << "] ref_dwrench = ["; + for (size_t i = 0; i < ref_dwrench.size(); i++) std::cerr << ref_dwrench[i] << ", "; + std::cerr << "], max_time = " << max_time << "[s]" << std::endl; + }; + // Setter + void setPrintStr (const std::string& str) { print_str = str; }; + void setWrenchCutoffFreq (const double a) + { + // All filters for wrench related values have the same cutoff freq. + wrench_filter->setCutOffFreq(a); + friction_coeff_wrench_filter->setCutOffFreq(a); + resultant_wrench_filter->setCutOffFreq(a); + }; + void setDwrenchCutoffFreq (const double a) + { + // All filters for dwrench related values have the same cutoff freq. + dwrench_filter->setCutOffFreq(a); + resultant_dwrench_filter->setCutOffFreq(a); + }; + // void setWrenchCutoffFreq (const double a) { wrench_filter->setCutOffFreq(a); }; + // void setDwrenchCutoffFreq (const double a) { dwrench_filter->setCutOffFreq(a); }; + // void setFrictionCoeffWrenchCutoffFreq (const double a) { friction_coeff_wrench_filter->setCutOffFreq(a); }; + void setDetectRatioThre (const double a) { detect_ratio_thre = a; }; + void setStartRatioThre (const double a) { start_ratio_thre = a; }; + void setDetectTimeThre (const double a) { detect_count_thre = round(a/dt); }; + void setStartTimeThre (const double a) { start_count_thre = round(a/dt); }; + void setOtherDetectTimeThre (const double a) { other_detect_count_thre = round(a/dt); }; + void setForgettingRatioThre (const double a) { forgetting_ratio_thre = a; }; + void setAxis (const hrp::Vector3& a) { axis = a; }; + void setMomentCenter (const hrp::Vector3& a) { moment_center = a; }; + void setConstraintConversionMatricesRefDwrench (const std::vector& ccm1, const std::vector& ccm2, const std::vector& refdw) + { + constraint_conversion_matrix1 = ccm1; + constraint_conversion_matrix2 = ccm2; + ref_dwrench = refdw; + resizeVariablesForGeneralizedWrench(constraint_conversion_matrix1.size()); + }; + void setDetectorTotalWrench (const detector_total_wrench _dtw) + { + if (_dtw != dtw) { + is_filter_reset = true; + } + dtw = _dtw; + }; + void setIsHoldValues (const bool a) { is_hold_values = a; }; + void setMaxTime (const double a) { max_time = a; }; + // Getter + double getWrenchCutoffFreq () const { return wrench_filter->getCutOffFreq(); }; + double getDwrenchCutoffFreq () const { return dwrench_filter->getCutOffFreq(); }; + //double getFrictionCoeffWrenchCutoffFreq () const { return friction_coeff_wrench_filter->getCutOffFreq(); }; + double getDetectRatioThre () const { return detect_ratio_thre; }; + double getStartRatioThre () const { return start_ratio_thre; }; + double getDetectTimeThre () const { return detect_count_thre*dt; }; + double getStartTimeThre () const { return start_count_thre*dt; }; + double getOtherDetectTimeThre () const { return other_detect_count_thre*dt; }; + double getForgettingRatioThre () const { return forgetting_ratio_thre; }; + hrp::Vector3 getAxis () const { return axis; }; + hrp::Vector3 getMomentCenter () const { return moment_center; }; + void getConstraintConversionMatricesRefDwrench (std::vector& ccm1, std::vector& ccm2, std::vector& refdw) const + { + ccm1 = constraint_conversion_matrix1; + ccm2 = constraint_conversion_matrix2; + refdw = ref_dwrench; + }; + detector_total_wrench getDetectorTotalWrench () const { return dtw; }; + process_mode getMode (const size_t idx) const { return pmode[idx]; }; + size_t getDetectGeneralizedWrenchDim () const { return pmode.size(); }; + hrp::dvector getDataForLogger () const + { + size_t data_size = 5; + hrp::dvector ret(pmode.size()*data_size); + for (size_t i = 0; i < pmode.size(); i++) { + size_t tmpoff = i*data_size; + ret(tmpoff) = static_cast(getMode(i)); + ret(1+tmpoff) = raw_wrench[i]; + ret(2+tmpoff) = phi1[i]; + ret(3+tmpoff) = dphi1[i]; + ret(4+tmpoff) = phi2[i]; + } + return ret; + }; + bool getIsHoldValues () const { return is_hold_values; }; + double getMaxTime () const { return max_time; }; + // For values with hold + std::vector getFilteredWrenchWithHold () const { return filtered_wrench_with_hold; }; + std::vector getFilteredFrictionCoeffWrenchWithHold () const { return filtered_friction_coeff_wrench_with_hold; }; + hrp::dvector6 getFilteredResultantWrenchWithHold () const { return filtered_resultant_wrench_with_hold; }; +}; +#endif // OBJECTCONTACTTURNAROUNDDETECTORBASE_H diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorComp.cpp b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorComp.cpp new file mode 100644 index 00000000000..238da4966f1 --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file ObjectContactTurnaroundDetectorComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "ObjectContactTurnaroundDetector.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + ObjectContactTurnaroundDetectorInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("ObjectContactTurnaroundDetector"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference +// RTC::RTObject_var rtobj; +// rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component +// PortList* portlist; +// portlist = rtobj->get_ports(); + + // getting port profiles +// std::cout << "Number of Ports: "; +// std::cout << portlist->length() << std::endl << std::endl; +// for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) +// { +// Port_ptr port; +// port = (*portlist)[i]; +// std::cout << "Port" << i << " (name): "; +// std::cout << port->get_port_profile()->name << std::endl; +// +// RTC::PortInterfaceProfileList iflist; +// iflist = port->get_port_profile()->interfaces; +// std::cout << "---interfaces---" << std::endl; +// for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) +// { +// std::cout << "I/F name: "; +// std::cout << iflist[i].instance_name << std::endl; +// std::cout << "I/F type: "; +// std::cout << iflist[i].type_name << std::endl; +// const char* pol; +// pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; +// std::cout << "Polarity: " << pol << std::endl; +// } +// std::cout << "---properties---" << std::endl; +// NVUtil::dump(port->get_port_profile()->properties); +// std::cout << "----------------" << std::endl << std::endl; +// } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorService_impl.cpp b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorService_impl.cpp new file mode 100644 index 00000000000..d764a15810b --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorService_impl.cpp @@ -0,0 +1,61 @@ +#include "ObjectContactTurnaroundDetectorService_impl.h" +#include "ObjectContactTurnaroundDetector.h" +#include +#include + +ObjectContactTurnaroundDetectorService_impl::ObjectContactTurnaroundDetectorService_impl() : m_octd(NULL) +{ +} + +ObjectContactTurnaroundDetectorService_impl::~ObjectContactTurnaroundDetectorService_impl() +{ +} + +void ObjectContactTurnaroundDetectorService_impl::startObjectContactTurnaroundDetection(const CORBA::Double i_ref_diff_wrench, const CORBA::Double i_max_time, const OpenHRP::ObjectContactTurnaroundDetectorService::StrSequence& i_ee_names) +{ + m_octd->startObjectContactTurnaroundDetection(i_ref_diff_wrench, i_max_time, i_ee_names); +} + +OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode ObjectContactTurnaroundDetectorService_impl::checkObjectContactTurnaroundDetection() +{ + return m_octd->checkObjectContactTurnaroundDetection(); +} + +CORBA::Boolean ObjectContactTurnaroundDetectorService_impl::setObjectContactTurnaroundDetectorParam(const OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam &i_param_) +{ + return m_octd->setObjectContactTurnaroundDetectorParam(i_param_); +} + +CORBA::Boolean ObjectContactTurnaroundDetectorService_impl::getObjectContactTurnaroundDetectorParam(OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam_out i_param_) +{ + i_param_ = new OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam(); + return m_octd->getObjectContactTurnaroundDetectorParam(*i_param_); +} + +CORBA::Boolean ObjectContactTurnaroundDetectorService_impl::getObjectForcesMoments(OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_forces, OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_moments, OpenHRP::ObjectContactTurnaroundDetectorService::DblSequence3_out o_3dofwrench, CORBA::Double& o_fric_coeff_wrench) +{ + return m_octd->getObjectForcesMoments(o_forces, o_moments, o_3dofwrench, o_fric_coeff_wrench); +} + +CORBA::Boolean ObjectContactTurnaroundDetectorService_impl::checkObjectContactTurnaroundDetectionForGeneralizedWrench(OpenHRP::ObjectContactTurnaroundDetectorService::DetectorModeSequence_out o_dms) +{ + o_dms = new OpenHRP::ObjectContactTurnaroundDetectorService::DetectorModeSequence(); + return m_octd->checkObjectContactTurnaroundDetectionForGeneralizedWrench(o_dms); +} + +CORBA::Boolean ObjectContactTurnaroundDetectorService_impl::startObjectContactTurnaroundDetectionForGeneralizedWrench() +{ + return m_octd->startObjectContactTurnaroundDetectionForGeneralizedWrench(); +} + +CORBA::Boolean ObjectContactTurnaroundDetectorService_impl::getObjectGeneralizedConstraintWrenches(OpenHRP::ObjectContactTurnaroundDetectorService::objectGeneralizedConstraintWrenchesParam_out o_param) +{ + o_param = new OpenHRP::ObjectContactTurnaroundDetectorService::objectGeneralizedConstraintWrenchesParam(); + return m_octd->getObjectGeneralizedConstraintWrenches(*o_param); +} + +void ObjectContactTurnaroundDetectorService_impl::octd(ObjectContactTurnaroundDetector *i_octd) +{ + m_octd = i_octd; +} + diff --git a/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorService_impl.h b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorService_impl.h new file mode 100644 index 00000000000..79f3ccf30f0 --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/ObjectContactTurnaroundDetectorService_impl.h @@ -0,0 +1,34 @@ +// -*-C++-*- +#ifndef OBJECTCONTACTTURNAROUNDDETECTORSERVICESVC_IMPL_H +#define OBJECTCONTACTTURNAROUNDDETECTORSERVICESVC_IMPL_H + +#include "hrpsys/idl/ObjectContactTurnaroundDetectorService.hh" + +using namespace OpenHRP; + +class ObjectContactTurnaroundDetector; + +class ObjectContactTurnaroundDetectorService_impl + : public virtual POA_OpenHRP::ObjectContactTurnaroundDetectorService, + public virtual PortableServer::RefCountServantBase +{ +public: + ObjectContactTurnaroundDetectorService_impl(); + virtual ~ObjectContactTurnaroundDetectorService_impl(); + // + void startObjectContactTurnaroundDetection(const CORBA::Double i_ref_diff_wrench, const CORBA::Double i_max_time, const OpenHRP::ObjectContactTurnaroundDetectorService::StrSequence& i_ee_names); + OpenHRP::ObjectContactTurnaroundDetectorService::DetectorMode checkObjectContactTurnaroundDetection(); + CORBA::Boolean setObjectContactTurnaroundDetectorParam(const OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam &i_param_); + CORBA::Boolean getObjectContactTurnaroundDetectorParam(OpenHRP::ObjectContactTurnaroundDetectorService::objectContactTurnaroundDetectorParam_out i_param_); + CORBA::Boolean getObjectForcesMoments(OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_forces, OpenHRP::ObjectContactTurnaroundDetectorService::Dbl3Sequence_out o_moments, OpenHRP::ObjectContactTurnaroundDetectorService::DblSequence3_out o_3dofwrench, CORBA::Double& o_fric_coeff_wrench); + CORBA::Boolean checkObjectContactTurnaroundDetectionForGeneralizedWrench(OpenHRP::ObjectContactTurnaroundDetectorService::DetectorModeSequence_out o_dms); + CORBA::Boolean startObjectContactTurnaroundDetectionForGeneralizedWrench(); + CORBA::Boolean getObjectGeneralizedConstraintWrenches(OpenHRP::ObjectContactTurnaroundDetectorService::objectGeneralizedConstraintWrenchesParam_out o_param); + + // + void octd(ObjectContactTurnaroundDetector *i_octd); +private: + ObjectContactTurnaroundDetector *m_octd; +}; + +#endif // OBJECTCONTACTTURNAROUNDDETECTORSERVICESVC_IMPL_H diff --git a/rtc/ObjectContactTurnaroundDetector/testObjectContactTurnaroundDetectorBase.cpp b/rtc/ObjectContactTurnaroundDetector/testObjectContactTurnaroundDetectorBase.cpp new file mode 100644 index 00000000000..bce08964fc1 --- /dev/null +++ b/rtc/ObjectContactTurnaroundDetector/testObjectContactTurnaroundDetectorBase.cpp @@ -0,0 +1,300 @@ +/* -*- coding:utf-8-unix; mode:c++; -*- */ + +#include "ObjectContactTurnaroundDetectorBase.h" +/* samples */ +#include +#include +#include +#include + +class testObjectContactTurnaroundDetectorBase +{ +protected: + double dt; /* [s] */ + ObjectContactTurnaroundDetectorBase octd; + bool use_gnuplot; + std::vector > forces_vec, moments_vec, hpos_vec; + double detect_time, true_turnaround_time; + std::vector time_vec; + void gen_pattern_and_plot () + { + parse_params(); + octd.printParams(); + std::string fname("/tmp/plot-octd.dat"); + FILE* fp = fopen(fname.c_str(), "w"); + bool detected = false; + double max_f = -1e10, min_f = 1e10; + for (size_t i = 0; i < time_vec.size();i++) { + bool tmp_detected = octd.checkDetection(forces_vec[i], moments_vec[i], hpos_vec[i]); + if (tmp_detected && !detected) { + detect_time = time_vec[i]; + detected = true; + } + hrp::dvector log_data = octd.getDataForLogger(); + fprintf(fp, "%f %f %f %f\n", time_vec[i], log_data[1], log_data[2], log_data[3], detected); + max_f = std::max(max_f, log_data[1]); + min_f = std::min(min_f, log_data[1]); + } + fclose(fp); + if (use_gnuplot) { + // plot + std::string titles[2] = {"Wrench", "Dwrench"}; + // plot pos + FILE* gp = popen("gnuplot", "w"); + fprintf(gp, "set multiplot layout 2, 1 title 'Results'\n"); + fprintf(gp, "set xlabel 'Time [s]'\n"); + fprintf(gp, "set ylabel 'Wrench'\n"); + fprintf(gp, "set arrow from %f,%f to %f,%f\n", detect_time, min_f, detect_time, max_f); + fprintf(gp, "plot '/tmp/plot-octd.dat' using 1:2 with lines title 'Wrench' lw 4, '/tmp/plot-octd.dat' using 1:3 with lines title 'FilteredWrench' lw 4\n"); + fprintf(gp, "unset arrow\n"); + fprintf(gp, "set xlabel 'Time [s]'\n"); + fprintf(gp, "set ylabel 'Dwrench'\n"); + fprintf(gp, "plot '/tmp/plot-octd.dat' using 1:4 with lines title 'Dwrench' lw 4\n"); + fflush(gp); + double tmp; + std::cin >> tmp; + pclose(gp); + } + }; +public: + std::vector arg_strs; + testObjectContactTurnaroundDetectorBase (const double _dt = 0.004) : dt(_dt), octd(_dt), use_gnuplot(true), detect_time(1e10), true_turnaround_time(1e10) + { + // Defaults + octd.setWrenchCutoffFreq(5.0); + octd.setDwrenchCutoffFreq(5.0); + octd.setAxis(hrp::Vector3::UnitZ()); + //octd.setDetectorTotalWrench(ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH); + //octd.setFrictionCoeffWrenchCutoffFreq(5.0); + }; + void gen_forces_moments (const std::vector& force_vec, const hrp::Vector3& force_dir = hrp::Vector3::UnitZ()) + { + for (size_t i = 0; i < force_vec.size();i++) { + std::vector tmpv(1, hrp::Vector3::Zero()); + moments_vec.push_back(tmpv); + hpos_vec.push_back(tmpv); + tmpv[0] = force_vec[i] * force_dir; + forces_vec.push_back(tmpv); + } + }; + hrp::dvector6 get_ccm1_by_index (size_t idx, bool is_positive = true) + { + hrp::dvector6 ccm1(hrp::dvector6::Zero()); + ccm1(idx) = (is_positive?1.0:-1.0); + return ccm1; + } + // convert object_resultant_wrench => constraint_generalized_force + double get_a_coeff_by_index (const double df, const hrp::dvector6& ccm1, const hrp::Vector3& fdir) + { + hrp::Vector3 fpos(0.2, 0.0, 0.7); // [m] + hrp::Vector3 tmp(fdir); + hrp::dvector6 resultant_wrench_direction_vector; + for (size_t i = 0; i < 3; i++) resultant_wrench_direction_vector(i) = tmp(i); + tmp = fpos.cross(fdir); + for (size_t i = 0; i < 3; i++) resultant_wrench_direction_vector(i+3) = tmp(i); + return -1*ccm1.dot(resultant_wrench_direction_vector) * df; + } + // Resultant force : robot's side resultant force + double gen_forces_moments_for_saturation (const double total_tm, const double start_tm, const double turnaround_tm, + const double start_resultant_force, const double turnaround_resultant_force, + const hrp::Vector3& force_dir = hrp::Vector3::UnitZ()) + { + std::vector phi_vec; + double dphi = (turnaround_resultant_force-start_resultant_force)/(turnaround_tm-start_tm); + true_turnaround_time = turnaround_tm; + for (size_t i = 0; i < static_cast(total_tm/dt);i++) { + double current_tm = i*dt; + time_vec.push_back(current_tm); + if (current_tm < start_tm) { + phi_vec.push_back(start_resultant_force); + } else if (current_tm < turnaround_tm) { + phi_vec.push_back(start_resultant_force+dphi*(current_tm-start_tm)); + } else { + phi_vec.push_back(turnaround_resultant_force); + } + } + gen_forces_moments(phi_vec, force_dir); + return dphi; + }; + double gen_forces_moments_for_inverting (const double total_tm, const double start_tm, const double turnaround_tm, + const double start_resultant_force, const double turnaround_resultant_force, + const hrp::Vector3& force_dir = hrp::Vector3::UnitZ()) + { + std::vector phi_vec; + double dphi = (turnaround_resultant_force-start_resultant_force)/(turnaround_tm-start_tm); + true_turnaround_time = turnaround_tm; + for (size_t i = 0; i < static_cast(total_tm/dt);i++) { + double current_tm = i*dt; + time_vec.push_back(current_tm); + if (current_tm < start_tm) { + phi_vec.push_back(start_resultant_force); + } else if (current_tm < turnaround_tm) { + phi_vec.push_back(start_resultant_force+dphi*(current_tm-start_tm)); + } else { + phi_vec.push_back(turnaround_resultant_force+(current_tm-turnaround_tm)*-2*dphi ); + } + } + gen_forces_moments(phi_vec, force_dir); + return dphi; + }; + void test0 () + { + std::cerr << "test0 : Increasing->saturation (TOTAL_FORCE)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = 40.0, dphi; + dphi = gen_forces_moments_for_saturation(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force); + octd.startDetection (dphi, total_tm); + gen_pattern_and_plot (); + }; + void test1 () + { + std::cerr << "test1 : Increasing->decreasing (TOTAL_FORCE)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = 40.0, dphi; + dphi = gen_forces_moments_for_inverting(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force); + octd.startDetection (dphi, total_tm); + gen_pattern_and_plot (); + }; + void test2 () + { + std::cerr << "test2 : Deacreasing->saturation (TOTAL_FORCE)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = -40.0, dphi; + dphi = gen_forces_moments_for_saturation(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force); + octd.startDetection (dphi, total_tm); + gen_pattern_and_plot (); + }; + void test3 () + { + std::cerr << "test3 : Decreasing->increasing (TOTAL_FORCE)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = -40.0, dphi; + dphi = gen_forces_moments_for_inverting(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force); + octd.startDetection (dphi, total_tm); + gen_pattern_and_plot (); + }; + void test4 () + { + std::cerr << "test4 : Lift up (GENERALIZED_WRENCH)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = -40.0, dphi; + dphi = std::fabs(gen_forces_moments_for_saturation(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force)); + octd.setDetectorTotalWrench(ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH); + std::vector ccm1(1, get_ccm1_by_index(2)); + double ref_dwrench = get_a_coeff_by_index(dphi, ccm1[0], hrp::Vector3::UnitZ()); + octd.setConstraintConversionMatricesRefDwrench(ccm1, + std::vector(1, hrp::dvector6::Zero()), + std::vector(1, ref_dwrench)); + octd.setMaxTime(total_tm); + octd.startDetectionForGeneralizedWrench(); + gen_pattern_and_plot (); + }; + void test5 () + { + std::cerr << "test5 : Push fwd (GENERALIZED_WRENCH)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = -40.0, dphi; + dphi = std::fabs(gen_forces_moments_for_saturation(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force, hrp::Vector3::UnitX())); + octd.setDetectorTotalWrench(ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH); + std::vector ccm1(1, get_ccm1_by_index(0)); + double ref_dwrench = get_a_coeff_by_index(dphi, ccm1[0], hrp::Vector3::UnitX()); + octd.setConstraintConversionMatricesRefDwrench(ccm1, + std::vector(1, hrp::dvector6::Zero()), + std::vector(1, ref_dwrench)); + octd.setMaxTime(total_tm); + octd.startDetectionForGeneralizedWrench(); + gen_pattern_and_plot (); + }; + void test6 () + { + std::cerr << "test6 : Push bwd (GENERALIZED_WRENCH)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = 40.0, dphi; + dphi = std::fabs(gen_forces_moments_for_saturation(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force, hrp::Vector3::UnitX())); + octd.setDetectorTotalWrench(ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH); + std::vector ccm1(1, get_ccm1_by_index(0, false)); + double ref_dwrench = get_a_coeff_by_index(dphi, ccm1[0], -1*hrp::Vector3::UnitX()); + octd.setConstraintConversionMatricesRefDwrench(ccm1, + std::vector(1, hrp::dvector6::Zero()), + std::vector(1, ref_dwrench)); + octd.setMaxTime(total_tm); + octd.startDetectionForGeneralizedWrench(); + gen_pattern_and_plot (); + }; + void test7 () + { + std::cerr << "test7 : Tilt upward (GENERALIZED_WRENCH)" << std::endl; + double total_tm = 4.0, start_tm = total_tm*0.1, turnaround_tm = total_tm*0.4, start_resultant_force = 0.0, turnaround_resultant_force = -40.0, dphi; + dphi = std::fabs(gen_forces_moments_for_saturation(total_tm, start_tm, turnaround_tm, start_resultant_force, turnaround_resultant_force)); + octd.setDetectorTotalWrench(ObjectContactTurnaroundDetectorBase::GENERALIZED_WRENCH); + std::vector ccm1(1, hrp::dvector6::Zero()); + ccm1[0](2) = 0.9; ccm1[0](4) = 1.0; + double ref_dwrench = get_a_coeff_by_index(dphi, ccm1[0], hrp::Vector3::UnitZ()); + octd.setConstraintConversionMatricesRefDwrench(ccm1, + std::vector(1, hrp::dvector6::Zero()), + std::vector(1, ref_dwrench)); + octd.setMaxTime(total_tm); + octd.startDetectionForGeneralizedWrench(); + gen_pattern_and_plot (); + }; + bool check_detection_time_validity (const double time_thre = 1.0) // [s] + { + return true_turnaround_time < detect_time && (detect_time-true_turnaround_time) < time_thre; + }; + bool check_all_results () + { + std::cerr << "Results:" << std::endl; + std::cerr << " Detected? : " << (octd.isDetected()?"true":"false") << std::endl; + std::cerr << " Detection time : " << (check_detection_time_validity()?"true":"false") << ", detect_time = " << detect_time << "[s], true_turnaround_time = " << true_turnaround_time << "[s]" << std::endl; + return octd.isDetected() && check_detection_time_validity(); + }; + void parse_params () + { + for (unsigned int i = 0; i < arg_strs.size(); ++ i) { + if ( arg_strs[i]== "--use-gnuplot" ) { + if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true"); + } + } + }; +}; + +void print_usage () +{ + std::cerr << "Usage : testObjectContactTurnaroundDetectorBase [option]" << std::endl; + std::cerr << " [option] should be:" << std::endl; + std::cerr << " --test0 : Increasing->saturation (TOTAL_FORCE)" << std::endl; + std::cerr << " --test1 : Increasing->saturation (GENERALIZED_WRENCH)" << std::endl; + std::cerr << " --test2 : Increasing->decreasing (GENERALIZED_WRENCH)" << std::endl; + std::cerr << " --test3 : Decreasing->saturation (GENERALIZED_WRENCH)" << std::endl; + std::cerr << " --test4 : Decreasing->increasing (GENERALIZED_WRENCH)" << std::endl; +}; + +int main(int argc, char* argv[]) +{ + int ret = 0; + if (argc >= 2) { + testObjectContactTurnaroundDetectorBase toctd; + for (int i = 1; i < argc; ++ i) { + toctd.arg_strs.push_back(std::string(argv[i])); + } + if (std::string(argv[1]) == "--test0") { + toctd.test0(); + } else if (std::string(argv[1]) == "--test1") { + toctd.test1(); + } else if (std::string(argv[1]) == "--test2") { + toctd.test2(); + } else if (std::string(argv[1]) == "--test3") { + toctd.test3(); + } else if (std::string(argv[1]) == "--test4") { + toctd.test4(); + } else if (std::string(argv[1]) == "--test5") { + toctd.test5(); + } else if (std::string(argv[1]) == "--test6") { + toctd.test6(); + } else if (std::string(argv[1]) == "--test7") { + toctd.test7(); + } else { + print_usage(); + ret = 1; + } + ret = (toctd.check_all_results()?0:2); + } else { + print_usage(); + ret = 1; + } + return ret; +} + diff --git a/rtc/OccupancyGridMap3D/CMakeLists.txt b/rtc/OccupancyGridMap3D/CMakeLists.txt index 995f36da554..04196f68c24 100644 --- a/rtc/OccupancyGridMap3D/CMakeLists.txt +++ b/rtc/OccupancyGridMap3D/CMakeLists.txt @@ -12,6 +12,6 @@ target_link_libraries(OccupancyGridMap3DComp ${libs}) set(target OccupancyGridMap3D OccupancyGridMap3DComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/OccupancyGridMap3D/OGMap3DService_impl.h b/rtc/OccupancyGridMap3D/OGMap3DService_impl.h index 71cb7a0f4f1..ae9eefd5112 100644 --- a/rtc/OccupancyGridMap3D/OGMap3DService_impl.h +++ b/rtc/OccupancyGridMap3D/OGMap3DService_impl.h @@ -2,7 +2,7 @@ #ifndef __OGMap3DService_H__ #define __OGMap3DService_H__ -#include "OGMap3DService.hh" +#include "hrpsys/idl/OGMap3DService.hh" class OccupancyGridMap3D; diff --git a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h index dc7f47c4cda..51c2f332549 100644 --- a/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h +++ b/rtc/OccupancyGridMap3D/OccupancyGridMap3D.h @@ -10,14 +10,15 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H +#include +#include +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include -#include "pointcloud.hh" namespace octomap{ class OcTree; diff --git a/rtc/OpenNIGrabber/CMakeLists.txt b/rtc/OpenNIGrabber/CMakeLists.txt new file mode 100644 index 00000000000..e647d3432ea --- /dev/null +++ b/rtc/OpenNIGrabber/CMakeLists.txt @@ -0,0 +1,19 @@ +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +set(comp_sources OpenNIGrabber.cpp) +set(libs hrpsysBaseStub ${PCL_LIBRARIES}) +add_library(OpenNIGrabber SHARED ${comp_sources}) +target_link_libraries(OpenNIGrabber ${libs}) +set_target_properties(OpenNIGrabber PROPERTIES PREFIX "") + +add_executable(OpenNIGrabberComp OpenNIGrabberComp.cpp ${comp_sources}) +target_link_libraries(OpenNIGrabberComp ${libs}) + +set(target OpenNIGrabber OpenNIGrabberComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/OpenNIGrabber/OpenNIGrabber.cpp b/rtc/OpenNIGrabber/OpenNIGrabber.cpp new file mode 100644 index 00000000000..d78e7a1f24c --- /dev/null +++ b/rtc/OpenNIGrabber/OpenNIGrabber.cpp @@ -0,0 +1,392 @@ +// -*- C++ -*- +/*! + * @file OpenNIGrabber.cpp + * @brief OpenNI grabber + * $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "OpenNIGrabber.h" +#include "hrpsys/idl/pointcloud.hh" + +// Module specification +// +static const char* spec[] = + { + "implementation_id", "OpenNIGrabber", + "type_name", "OpenNIGrabber", + "description", "OpenNI grabber", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.outputColorImage", "0", + "conf.default.outputDepthImage", "0", + "conf.default.outputPointCloud", "0", + "conf.default.outputPointCloudRGBA", "0", + "conf.default.debugLevel", "0", + + "" + }; +// + +OpenNIGrabber::OpenNIGrabber(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_cloudOut("cloud", m_cloud), + m_imageOut("image", m_image), + m_depthOut("depth", m_depth), + // + m_interface(NULL), + m_requestToWriteImage(false), + m_requestToWritePointCloud(false), + dummy(0) +{ +} + +OpenNIGrabber::~OpenNIGrabber() +{ +} + + + +RTC::ReturnCode_t OpenNIGrabber::onInitialize() +{ + std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("outputColorImage", m_outputColorImage, "0"); + bindParameter("outputDepthImage", m_outputDepthImage, "0"); + bindParameter("outputPointCloud", m_outputPointCloud, "0"); + bindParameter("outputPointCloudRGBA", m_outputPointCloudRGBA, "0"); + bindParameter("debugLevel", m_debugLevel, "0"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + + // Set OutPort buffer + addOutPort("cloudOut", m_cloudOut); + addOutPort("imageOut", m_imageOut); + addOutPort("depthOut", m_depthOut); + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + RTC::Properties& prop = getProperties(); + + return RTC::RTC_OK; +} + + +void OpenNIGrabber::grabberCallbackColorImage(const boost::shared_ptr& image) +{ + if (!m_requestToWriteImage) return; + + outputColorImage(image); + + m_requestToWriteImage = false; +} + +void OpenNIGrabber::outputColorImage(const boost::shared_ptr& image) +{ + setTimestamp(m_image); + + Img::ImageData &id = m_image.data.image; + id.width = image->getWidth(); + id.height = image->getHeight(); + id.format = Img::CF_RGB; + id.raw_data.length(id.width*id.height*3); + + unsigned char *dst = (unsigned char*)id.raw_data.get_buffer(); + unsigned char *src = (unsigned char*)image->getData(); + memcpy(dst, src, id.raw_data.length()); + + m_imageOut.write(); +} + +void OpenNIGrabber::grabberCallbackDepthImage(const boost::shared_ptr& image) +{ + if (!m_requestToWriteImage) return; + + outputDepthImage(image); + + m_requestToWriteImage = false; +} + +void OpenNIGrabber::outputDepthImage(const boost::shared_ptr& image) +{ + setTimestamp(m_depth); + + Img::ImageData &id = m_depth.data.image; + id.width = image->getWidth(); + id.height = image->getHeight(); + id.format = Img::CF_DEPTH; + id.raw_data.length(id.width*id.height*2); + + unsigned char *dst = (unsigned char*)id.raw_data.get_buffer(); + unsigned char *src = (unsigned char*)image->getData(); + memcpy(dst, src, id.raw_data.length()); + + m_depthOut.write(); +} + +void OpenNIGrabber::grabberCallbackColorAndDepthImage(const boost::shared_ptr& image, const boost::shared_ptr& depth, float reciprocalFocalLength) +{ + if (!m_requestToWriteImage) return; + + outputColorImage(image); + outputDepthImage(depth); + + m_requestToWriteImage = false; +} + +void OpenNIGrabber::grabberCallbackPointCloudRGBA(const pcl::PointCloud::ConstPtr &cloud) +{ + if (!m_requestToWritePointCloud) return; + + setTimestamp(m_cloud); + + m_cloud.width = cloud->width; + m_cloud.height = cloud->height; + m_cloud.row_step = m_cloud.point_step*m_cloud.width; + m_cloud.data.length(m_cloud.height*m_cloud.row_step); + + float *dst_cloud = (float *)m_cloud.data.get_buffer(); + for (unsigned int i=0; ipoints.size(); i++){ + dst_cloud[0] = cloud->points[i].x; + dst_cloud[1] = -cloud->points[i].y; + dst_cloud[2] = -cloud->points[i].z; + unsigned char *rgb = (unsigned char *)(dst_cloud+3); + rgb[0] = cloud->points[i].r; + rgb[1] = cloud->points[i].g; + rgb[2] = cloud->points[i].b; + dst_cloud += 4; + } + m_cloudOut.write(); + m_requestToWritePointCloud = false; +} + +void OpenNIGrabber::grabberCallbackPointCloud(const pcl::PointCloud::ConstPtr &cloud) +{ + if (!m_requestToWritePointCloud) return; + + setTimestamp(m_cloud); + m_cloud.width = cloud->width; + m_cloud.height = cloud->height; + m_cloud.row_step = m_cloud.point_step*m_cloud.width; + m_cloud.data.length(m_cloud.height*m_cloud.row_step); + + float *dst_cloud = (float *)m_cloud.data.get_buffer(); + for (unsigned int i=0; ipoints.size(); i++){ + dst_cloud[0] = cloud->points[i].x; + dst_cloud[1] = -cloud->points[i].y; + dst_cloud[2] = -cloud->points[i].z; + dst_cloud += 4; + } + m_cloudOut.write(); + m_requestToWritePointCloud = false; +} + +/* +RTC::ReturnCode_t OpenNIGrabber::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t OpenNIGrabber::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t OpenNIGrabber::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t OpenNIGrabber::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + + try { + m_interface = new pcl::io::OpenNI2Grabber(); + + if (m_outputColorImage && !m_outputDepthImage){ + boost::function&)> f = boost::bind(&OpenNIGrabber::grabberCallbackColorImage, this, _1); + m_interface->registerCallback(f); + }else if (!m_outputColorImage && m_outputDepthImage){ + boost::function&)> f = boost::bind(&OpenNIGrabber::grabberCallbackDepthImage, this, _1); + m_interface->registerCallback(f); + }else if (m_outputColorImage && m_outputDepthImage){ + boost::function&, const boost::shared_ptr&, float)> f = boost::bind(&OpenNIGrabber::grabberCallbackColorAndDepthImage, this, _1, _2, _3); + m_interface->registerCallback(f); + } + if (m_outputPointCloud){ + m_cloud.type = "xyz"; + m_cloud.fields.length(3); + m_cloud.fields[0].name = "x"; + m_cloud.fields[0].offset = 0; + m_cloud.fields[0].data_type = PointCloudTypes::FLOAT32; + m_cloud.fields[0].count = 4; + m_cloud.fields[1].name = "y"; + m_cloud.fields[1].offset = 4; + m_cloud.fields[1].data_type = PointCloudTypes::FLOAT32; + m_cloud.fields[1].count = 4; + m_cloud.fields[2].name = "z"; + m_cloud.fields[2].offset = 8; + m_cloud.fields[2].data_type = PointCloudTypes::FLOAT32; + m_cloud.fields[2].count = 4; + m_cloud.is_bigendian = false; + m_cloud.point_step = 16; + m_cloud.is_dense = false; + boost::function::ConstPtr&)> f = boost::bind(&OpenNIGrabber::grabberCallbackPointCloud, this, _1); + m_interface->registerCallback(f); + }else if(m_outputPointCloudRGBA){ + m_cloud.type = "xyzrgb"; + m_cloud.fields.length(6); + m_cloud.fields[0].name = "x"; + m_cloud.fields[0].offset = 0; + m_cloud.fields[0].data_type = PointCloudTypes::FLOAT32; + m_cloud.fields[0].count = 4; + m_cloud.fields[1].name = "y"; + m_cloud.fields[1].offset = 4; + m_cloud.fields[1].data_type = PointCloudTypes::FLOAT32; + m_cloud.fields[1].count = 4; + m_cloud.fields[2].name = "z"; + m_cloud.fields[2].offset = 8; + m_cloud.fields[2].data_type = PointCloudTypes::FLOAT32; + m_cloud.fields[2].count = 4; + m_cloud.fields[3].name = "r"; + m_cloud.fields[3].offset = 12; + m_cloud.fields[3].data_type = PointCloudTypes::UINT8; + m_cloud.fields[3].count = 1; + m_cloud.fields[4].name = "g"; + m_cloud.fields[4].offset = 13; + m_cloud.fields[4].data_type = PointCloudTypes::UINT8; + m_cloud.fields[4].count = 1; + m_cloud.fields[5].name = "b"; + m_cloud.fields[5].offset = 14; + m_cloud.fields[5].data_type = PointCloudTypes::UINT8; + m_cloud.fields[5].count = 1; + m_cloud.is_bigendian = false; + m_cloud.point_step = 16; + m_cloud.is_dense = false; + boost::function::ConstPtr&)> f = boost::bind(&OpenNIGrabber::grabberCallbackPointCloudRGBA, this, _1); + m_interface->registerCallback(f); + } + + m_interface->start(); + }catch(pcl::IOException& ex){ + std::cerr << "[" << m_profile.instance_name << "] Error: " << ex.what() << std::endl; + return RTC::RTC_ERROR; + }catch(...){ + std::cerr << "[" << m_profile.instance_name + << "] Error: An exception occurred while starting grabber" << std::endl; + return RTC::RTC_ERROR; + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t OpenNIGrabber::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + + if (m_interface){ + m_interface->stop(); + + delete m_interface; + } + + return RTC::RTC_OK; +} + +RTC::ReturnCode_t OpenNIGrabber::onExecute(RTC::UniqueId ec_id) +{ + //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + if (m_debugLevel>0){ + if (m_interface->isRunning()){ + std::cerr << "[" << m_profile.instance_name + << "] grabber is running. frame rate=" << m_interface->getFramesPerSecond() << std::endl; + }else{ + std::cerr << "[" << m_profile.instance_name + << "] grabber is not running" << std::endl; + } + } + m_requestToWriteImage = true; + m_requestToWritePointCloud = true; + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t OpenNIGrabber::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t OpenNIGrabber::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t OpenNIGrabber::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t OpenNIGrabber::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t OpenNIGrabber::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + + + +extern "C" +{ + + void OpenNIGrabberInit(RTC::Manager* manager) + { + RTC::Properties profile(spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/OpenNIGrabber/OpenNIGrabber.h b/rtc/OpenNIGrabber/OpenNIGrabber.h new file mode 100644 index 00000000000..286de9cde97 --- /dev/null +++ b/rtc/OpenNIGrabber/OpenNIGrabber.h @@ -0,0 +1,166 @@ +// -*- C++ -*- +/*! + * @file OpenNIGrabber.h + * @brief Moving Least Squares Filter + * @date $Date$ + * + * $Id$ + */ + +#ifndef OPENNI_GRABBER_H +#define OPENNI_GRABBER_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include "hrpsys/idl/pointcloud.hh" +#include "hrpsys/idl/Img.hh" + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief sample RT component which has one data input port and one data output port + */ +class OpenNIGrabber + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + OpenNIGrabber(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~OpenNIGrabber(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + Img::TimedCameraImage m_image; + Img::TimedCameraImage m_depth; + PointCloudTypes::PointCloud m_cloud; + + // DataInPort declaration + // + + // + + // DataOutPort declaration + // + OutPort m_cloudOut; + OutPort m_imageOut; + OutPort m_depthOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + void grabberCallbackColorImage(const boost::shared_ptr& image); + void grabberCallbackDepthImage(const boost::shared_ptr& image); + void grabberCallbackColorAndDepthImage(const boost::shared_ptr& image, const boost::shared_ptr& depth, float reciprocalFocalLength); + void grabberCallbackPointCloudRGBA(const pcl::PointCloud::ConstPtr &cloud); + void grabberCallbackPointCloud(const pcl::PointCloud::ConstPtr &cloud); + void outputColorImage(const boost::shared_ptr& image); + void outputDepthImage(const boost::shared_ptr& image); + + pcl::Grabber *m_interface; + int m_debugLevel; + bool m_outputColorImage; + bool m_outputDepthImage; + bool m_outputPointCloud; + bool m_outputPointCloudRGBA; + bool m_requestToWriteImage; + bool m_requestToWritePointCloud; + int dummy; +}; + + +extern "C" +{ + void OpenNIGrabberInit(RTC::Manager* manager); +}; + +#endif // OPENNI_GRABBER_H diff --git a/rtc/OpenNIGrabber/OpenNIGrabber.txt b/rtc/OpenNIGrabber/OpenNIGrabber.txt new file mode 100644 index 00000000000..dfa422304bd --- /dev/null +++ b/rtc/OpenNIGrabber/OpenNIGrabber.txt @@ -0,0 +1,54 @@ +/** + +\page OpenNIGrabber + +\section introduction Overview + +This component outputs data obtained by a RGBD sensor. + + + + +
    implementation_idOpenNIGrabber
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + +N/A + +\subsection outports Output Ports + + + + + + +
    port namedata typeunitdescription
    imageImg::TimedCameraImagecolor image
    depthImg::TimedCameraImagedepth image
    cloudPointCloudTypes::PointCloudPoint cloud
    + +\section serviceports Service Ports + +\subsection provider Service Providers + +N/A + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + + + + + + +
    nametypeunitdefault valuedescription
    outputColorImagebool0flag to output color images
    outputDepthImagebool0flag to output depth images
    outputPointCloudbool0flag to output point clouds (XYZ format)
    outputPointCloudRGBAbool0flag to output point clouds (XYZRGBA format)
    debugLevelint0debug level
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/OpenNIGrabber/OpenNIGrabberComp.cpp b/rtc/OpenNIGrabber/OpenNIGrabberComp.cpp new file mode 100644 index 00000000000..b2089e283db --- /dev/null +++ b/rtc/OpenNIGrabber/OpenNIGrabberComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file OpenNIGrabberComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "OpenNIGrabber.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + OpenNIGrabberInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("OpenNIGrabber"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/PCDLoader/CMakeLists.txt b/rtc/PCDLoader/CMakeLists.txt index 6561e0325c9..873bd198d62 100644 --- a/rtc/PCDLoader/CMakeLists.txt +++ b/rtc/PCDLoader/CMakeLists.txt @@ -2,8 +2,8 @@ include_directories(${PCL_INCLUDE_DIRS}) link_directories(${PCL_LIBRARY_DIRS}) add_definitions(${PCL_DEFINITIONS}) -set(comp_sources PCDLoader.cpp) -set(libs hrpsysBaseStub ${PCL_LIBRARIES}) +set(comp_sources PCDLoader.cpp PCDLoaderService_impl.cpp) +set(libs hrpsysBaseStub ${OPENHRP_LIBRARIES} ${PCL_LIBRARIES}) add_library(PCDLoader SHARED ${comp_sources}) target_link_libraries(PCDLoader ${libs}) set_target_properties(PCDLoader PROPERTIES PREFIX "") @@ -14,6 +14,6 @@ target_link_libraries(PCDLoaderComp ${libs}) set(target PCDLoader PCDLoaderComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/PCDLoader/PCDLoader.cpp b/rtc/PCDLoader/PCDLoader.cpp index e81a44ca19c..116399e9688 100644 --- a/rtc/PCDLoader/PCDLoader.cpp +++ b/rtc/PCDLoader/PCDLoader.cpp @@ -7,11 +7,10 @@ * $Id$ */ -#include #include #include #include "PCDLoader.h" -#include "pointcloud.hh" +#include "hrpsys/idl/pointcloud.hh" // Module specification // @@ -28,6 +27,8 @@ static const char* spec[] = "language", "C++", "lang_type", "compile", // Configuration variables + "conf.default.path", "", + "conf.default.fields","XYZ", "" }; @@ -36,8 +37,11 @@ static const char* spec[] = PCDLoader::PCDLoader(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // + m_offsetIn("offset", m_offset), + m_isOutputOut("isOutput", m_isOutput), m_cloudOut("cloud", m_cloud), // + m_PCDLoaderServicePort("PCDLoaderService"), dummy(0) { } @@ -50,52 +54,193 @@ PCDLoader::~PCDLoader() RTC::ReturnCode_t PCDLoader::onInitialize() { - //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; - // - // Bind variables and configuration variable - - // - - // Registration: InPort/OutPort/Service - // - // Set InPort buffers - - // Set OutPort buffer - addOutPort("cloudOut", m_cloudOut); - - // Set service provider to Ports - - // Set service consumers to Ports - - // Set CORBA Service Ports - - // - - RTC::Properties& prop = getProperties(); - - m_cloud.height = 1; - m_cloud.type = "xyz"; - m_cloud.fields.length(3); - m_cloud.fields[0].name = "x"; - m_cloud.fields[0].offset = 0; - m_cloud.fields[0].data_type = PointCloudTypes::FLOAT32; - m_cloud.fields[0].count = 4; - m_cloud.fields[1].name = "y"; - m_cloud.fields[1].offset = 4; - m_cloud.fields[1].data_type = PointCloudTypes::FLOAT32; - m_cloud.fields[1].count = 4; - m_cloud.fields[2].name = "z"; - m_cloud.fields[2].offset = 8; - m_cloud.fields[2].data_type = PointCloudTypes::FLOAT32; - m_cloud.fields[2].count = 4; - m_cloud.is_bigendian = false; - m_cloud.point_step = 16; - m_cloud.is_dense = true; + //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("path", m_path, ""); + bindParameter("fields", m_fields, "XYZ"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("offsetIn", m_offsetIn); + + // Set OutPort buffer + addOutPort("isOutputOut", m_isOutputOut); + addOutPort("cloudOut", m_cloudOut); + + // Set service provider to Ports + m_PCDLoaderServicePort.registerProvider("service0", "PCDLoaderService", m_service0); + addPort(m_PCDLoaderServicePort); + m_service0.setComp(this); + m_isOutput.data = true; + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + RTC::Properties& prop = getProperties(); + + return RTC::RTC_OK; +} - return RTC::RTC_OK; +void PCDLoader::setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud::Ptr cloud_raw) +{ + int npoint = cloud_raw->points.size(); + + cloud.type = "xyz"; + cloud.fields.length(3); + cloud.fields[0].name = "x"; + cloud.fields[0].offset = 0; + cloud.fields[0].data_type = PointCloudTypes::FLOAT32; + cloud.fields[0].count = 4; + cloud.fields[1].name = "y"; + cloud.fields[1].offset = 4; + cloud.fields[1].data_type = PointCloudTypes::FLOAT32; + cloud.fields[1].count = 4; + cloud.fields[2].name = "z"; + cloud.fields[2].offset = 8; + cloud.fields[2].data_type = PointCloudTypes::FLOAT32; + cloud.fields[2].count = 4; + cloud.is_bigendian = false; + cloud.point_step = 16; + cloud.width = cloud_raw->width; + cloud.height = cloud_raw->height; + cloud.data.length(npoint*cloud.point_step); + cloud.row_step = cloud.width*cloud.point_step; + cloud.is_dense = cloud_raw->is_dense; + float *ptr = (float *)cloud.data.get_buffer(); + std::cout << "npoint = " << npoint << std::endl; + for (int i=0; ipoints[i].x; + ptr[1] = cloud_raw->points[i].y; + ptr[2] = cloud_raw->points[i].z; + ptr += 4; + } } +void PCDLoader::setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud::Ptr cloud_raw) +{ + int npoint = cloud_raw->points.size(); + + cloud.type = "xyzrgb"; + cloud.fields.length(6); + cloud.fields[0].name = "x"; + cloud.fields[0].offset = 0; + cloud.fields[0].data_type = PointCloudTypes::FLOAT32; + cloud.fields[0].count = 4; + cloud.fields[1].name = "y"; + cloud.fields[1].offset = 4; + cloud.fields[1].data_type = PointCloudTypes::FLOAT32; + cloud.fields[1].count = 4; + cloud.fields[2].name = "z"; + cloud.fields[2].offset = 8; + cloud.fields[2].data_type = PointCloudTypes::FLOAT32; + cloud.fields[2].count = 4; + cloud.fields[3].name = "r"; + cloud.fields[3].offset = 12; + cloud.fields[3].data_type = PointCloudTypes::UINT8; + cloud.fields[3].count = 1; + cloud.fields[4].name = "g"; + cloud.fields[4].offset = 13; + cloud.fields[4].data_type = PointCloudTypes::UINT8; + cloud.fields[4].count = 1; + cloud.fields[5].name = "b"; + cloud.fields[5].offset = 14; + cloud.fields[5].data_type = PointCloudTypes::UINT8; + cloud.fields[5].count = 1; + cloud.is_bigendian = false; + cloud.point_step = 16; + cloud.width = cloud_raw->width; + cloud.height = cloud_raw->height; + cloud.data.length(npoint*cloud.point_step); + cloud.row_step = cloud.width*cloud.point_step; + cloud.is_dense = cloud_raw->is_dense; + float *ptr = (float *)cloud.data.get_buffer(); + std::cout << "npoint = " << npoint << std::endl; + for (int i=0; ipoints[i].x; + ptr[1] = cloud_raw->points[i].y; + ptr[2] = cloud_raw->points[i].z; + unsigned char *rgb = (unsigned char *)(ptr+3); + rgb[0] = cloud_raw->points[i].r; + rgb[1] = cloud_raw->points[i].g; + rgb[2] = cloud_raw->points[i].b; + ptr += 4; + } +} +void PCDLoader::updateOffsetToCloudXYZ(void) +{ + pcl::PointCloud::Ptr clouds(new pcl::PointCloud); + for (unsigned int i=0; i::Ptr cloud_raw = m_clouds_xyz[label]; + pcl::PointCloud::Ptr cloud_new(new pcl::PointCloud); + int npoint = cloud_raw->points.size(); + cloud_new->points.resize(npoint); + cloud_new->width = cloud_raw->width; + cloud_new->height = cloud_raw->height; + cloud_new->is_dense = cloud_raw->is_dense; + hrp::Vector3 point, point_new; + for(int j = 0 ; j < npoint ; j++){ + point << cloud_raw->points[j].x, cloud_raw->points[j].y, cloud_raw->points[j].z; + point_new = offsetR * (point - center) + center + offsetP; + cloud_new->points[j].x = point_new[0]; + cloud_new->points[j].y = point_new[1]; + cloud_new->points[j].z = point_new[2]; + } + *clouds += *cloud_new; + } + } + setCloudXYZ(m_cloud, clouds); +} + +void PCDLoader::updateOffsetToCloudXYZRGB(void) +{ + pcl::PointCloud::Ptr clouds(new pcl::PointCloud); + for (unsigned int i=0; i::Ptr cloud_raw = m_clouds_xyzrgb[label]; + pcl::PointCloud::Ptr cloud_new(new pcl::PointCloud); + int npoint = cloud_raw->points.size(); + cloud_new->points.resize(npoint); + cloud_new->width = cloud_raw->width; + cloud_new->height = cloud_raw->height; + cloud_new->is_dense = cloud_raw->is_dense; + hrp::Vector3 point, point_new; + for(int j = 0 ; j < npoint ; j++){ + point << cloud_raw->points[j].x, cloud_raw->points[j].y, cloud_raw->points[j].z; + point_new = offsetR * (point - center) + center + offsetP; + cloud_new->points[j].x = point_new[0]; + cloud_new->points[j].y = point_new[1]; + cloud_new->points[j].z = point_new[2]; + } + *clouds += *cloud_new; + } + } + setCloudXYZRGB(m_cloud, clouds); +} /* RTC::ReturnCode_t PCDLoader::onFinalize() @@ -120,44 +265,63 @@ RTC::ReturnCode_t PCDLoader::onShutdown(RTC::UniqueId ec_id) RTC::ReturnCode_t PCDLoader::onActivated(RTC::UniqueId ec_id) { - std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; - return RTC::RTC_OK; + std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; } RTC::ReturnCode_t PCDLoader::onDeactivated(RTC::UniqueId ec_id) { - std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; - return RTC::RTC_OK; + std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; } RTC::ReturnCode_t PCDLoader::onExecute(RTC::UniqueId ec_id) { - //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; - std::cerr << "PCD filename: " << std::flush; - std::string filename; - std::cin >> filename; - - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - - pcl::PCDReader reader; - reader.read (filename, *cloud); - int npoint = cloud->points.size(); - m_cloud.width = npoint; - m_cloud.height = 1; - m_cloud.data.length(npoint*m_cloud.point_step); - m_cloud.row_step = m_cloud.data.length(); - float *ptr = (float *)m_cloud.data.get_buffer(); - std::cout << "npoint = " << npoint << std::endl; - for (int i=0; ipoints[i].x; - ptr[1] = cloud->points[i].y; - ptr[2] = cloud->points[i].z; - ptr += 4; - } - - m_cloudOut.write(); - - return RTC::RTC_OK; + //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + if ( !m_path.empty()){ + pcl::PCDReader reader; + + if (m_fields=="XYZ"){ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if (reader.read (m_path, *cloud)){ + std::cerr << m_profile.instance_name << ": failed to load(" + << m_path << ")" << std::endl; + m_path = ""; + return RTC::RTC_OK; + } + setCloudXYZ(m_cloud, cloud); + }else if(m_fields=="XYZRGB"){ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if (reader.read (m_path, *cloud)){ + std::cerr << m_profile.instance_name << ": failed to load(" + << m_path << ")" << std::endl; + m_path = ""; + return RTC::RTC_OK; + } + setCloudXYZRGB(m_cloud, cloud); + }else{ + std::cerr << "fields[" << m_fields << "] is not supported" << std::endl; + } + m_cloudOut.write(); + m_isOutputOut.write(); + m_path = ""; + } + + if( m_offsetIn.isNew() ){ + m_offsetIn.read(); + if( !m_clouds_xyz.empty() ){ + updateOffsetToCloudXYZ(); + m_cloudOut.write(); + m_isOutputOut.write(); + } + else if( !m_clouds_xyzrgb.empty() ){ + updateOffsetToCloudXYZRGB(); + m_cloudOut.write(); + m_isOutputOut.write(); + } + } + + return RTC::RTC_OK; } /* @@ -196,18 +360,46 @@ RTC::ReturnCode_t PCDLoader::onRateChanged(RTC::UniqueId ec_id) */ +bool PCDLoader::load(const std::string& filename, const std::string& label) +{ + pcl::PCDReader reader; + if( m_fields == "XYZ" ){ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if (reader.read (filename, *cloud)){ + std::cerr << m_profile.instance_name << ": failed to load(" + << filename << ")" << std::endl; + return RTC::RTC_OK; + } + else + std::cout << "Loading " << filename << " with XYZ fields." << std::endl; + m_clouds_xyz[label] = cloud; + } + else if( m_fields == "XYZRGB" ){ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + if (reader.read (filename, *cloud)){ + std::cerr << m_profile.instance_name << ": failed to load(" + << filename << ")" << std::endl; + return RTC::RTC_OK; + } + else + std::cout << "Loading " << filename << " with XYZRGB fields." << std::endl; + m_clouds_xyzrgb[label] = cloud; + }else{ + std::cerr << "fields[" << m_fields << "] is not supported" << std::endl; + } +} extern "C" { - - void PCDLoaderInit(RTC::Manager* manager) - { - RTC::Properties profile(spec); - manager->registerFactory(profile, - RTC::Create, - RTC::Delete); - } - + + void PCDLoaderInit(RTC::Manager* manager) + { + RTC::Properties profile(spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + }; diff --git a/rtc/PCDLoader/PCDLoader.h b/rtc/PCDLoader/PCDLoader.h index a24bc092b96..729199a92ff 100644 --- a/rtc/PCDLoader/PCDLoader.h +++ b/rtc/PCDLoader/PCDLoader.h @@ -10,13 +10,19 @@ #ifndef PCD_LOADER_H #define PCD_LOADER_H +#include +#include "hrpsys/idl/pointcloud.hh" +#include "hrpsys/idl/PCDLoaderService.hh" #include #include #include #include #include #include -#include "pointcloud.hh" +#include +#include +#include +#include "PCDLoaderService_impl.h" // Service implementation headers // @@ -94,34 +100,42 @@ class PCDLoader // The action that is invoked when execution context's rate is changed // no corresponding operation exists in OpenRTm-aist-0.2.0 // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); - - - protected: + + bool load(const std::string& filename, const std::string& label); + +protected: // Configuration variable declaration // // - + PointCloudTypes::PointCloud m_cloud; - + OpenHRP::PCDOffsetSeq m_offset; + RTC::TimedBoolean m_isOutput; + // DataInPort declaration // + InPort m_offsetIn; + // - + // DataOutPort declaration // OutPort m_cloudOut; + OutPort m_isOutputOut; // // CORBA Port declaration // + RTC::CorbaPort m_PCDLoaderServicePort; // // Service declaration // + PCDLoaderService_impl m_service0; // @@ -130,8 +144,19 @@ class PCDLoader // + void setCloudXYZ(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud::Ptr cloud_raw); + + void setCloudXYZRGB(PointCloudTypes::PointCloud& cloud, const pcl::PointCloud::Ptr cloud_raw); + + void updateOffsetToCloudXYZ(void); + + void updateOffsetToCloudXYZRGB(void); + private: + std::string m_path, m_fields; int dummy; + boost::unordered_map::Ptr> m_clouds_xyz; + boost::unordered_map::Ptr> m_clouds_xyzrgb; }; diff --git a/rtc/PCDLoader/PCDLoaderService_impl.cpp b/rtc/PCDLoader/PCDLoaderService_impl.cpp new file mode 100644 index 00000000000..a8a65e12c64 --- /dev/null +++ b/rtc/PCDLoader/PCDLoaderService_impl.cpp @@ -0,0 +1,21 @@ +#include "PCDLoaderService_impl.h" +#include "PCDLoader.h" + +PCDLoaderService_impl::PCDLoaderService_impl() : m_comp(NULL) +{ +} + +PCDLoaderService_impl::~PCDLoaderService_impl() +{ +} + + +void PCDLoaderService_impl::setComp(PCDLoader *i_comp) +{ + m_comp = i_comp; +} + +::CORBA::Boolean PCDLoaderService_impl::load(const char* filename, const char* label) +{ + return m_comp->load(filename, label); +} diff --git a/rtc/PCDLoader/PCDLoaderService_impl.h b/rtc/PCDLoader/PCDLoaderService_impl.h new file mode 100644 index 00000000000..708e0c265fa --- /dev/null +++ b/rtc/PCDLoader/PCDLoaderService_impl.h @@ -0,0 +1,30 @@ +#ifndef __PCD_LOADER_SERVICE_IMPL_H__ +#define __PCD_LOADER_SERVICE_IMPL_H__ + +#include "hrpsys/idl/PCDLoaderService.hh" + +class PCDLoader; + +class PCDLoaderService_impl + : public virtual POA_OpenHRP::PCDLoaderService, + public virtual PortableServer::RefCountServantBase +{ + public: + /** + \brief constructor + */ + PCDLoaderService_impl(); + + /** + \brief destructor + */ + virtual ~PCDLoaderService_impl(); + + void setComp(PCDLoader *i_comp); + // + ::CORBA::Boolean load(const char* filename, const char* label); + private: + PCDLoader *m_comp; +}; + +#endif diff --git a/rtc/PCDLoader/test.py b/rtc/PCDLoader/test.py new file mode 100644 index 00000000000..99ba4436983 --- /dev/null +++ b/rtc/PCDLoader/test.py @@ -0,0 +1,19 @@ +import rtm + +rtm.nsport=2809 +rtm.initCORBA() + +mgr = rtm.findRTCmanager() + +mgr.load("PCDLoader") +mgr.load("PointCloudViewer") + +pcl = mgr.create("PCDLoader") +pcv = mgr.create("PointCloudViewer") + +rtm.connectPorts(pcl.port("cloud"), pcv.port("cloud")) + +pcl.setProperty("fields", "XYZRGB") +pcl.start() +pcv.start() + diff --git a/rtc/PDcontroller/CMakeLists.txt b/rtc/PDcontroller/CMakeLists.txt index c6af5cfa01f..a8b4c25ee9c 100644 --- a/rtc/PDcontroller/CMakeLists.txt +++ b/rtc/PDcontroller/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(PDcontrollerComp ${libs}) set(target PDcontroller PDcontrollerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/PDcontroller/PDcontroller.cpp b/rtc/PDcontroller/PDcontroller.cpp index 885c45cd479..47cff336d23 100644 --- a/rtc/PDcontroller/PDcontroller.cpp +++ b/rtc/PDcontroller/PDcontroller.cpp @@ -40,9 +40,9 @@ PDcontroller::PDcontroller(RTC::Manager* manager) m_torqueOut("torque", m_torque), dt(0.005), // - dummy(0), gain_fname(""), - dof(0), loop(0) + dof(0), loop(0), + dummy(0) { } @@ -57,6 +57,10 @@ RTC::ReturnCode_t PDcontroller::onInitialize() RTC::Properties& prop = getProperties(); coil::stringTo(dt, prop["dt"].c_str()); + ref_dt = dt; + coil::stringTo(ref_dt, prop["ref_dt"].c_str()); + nstep = ref_dt/dt; + step = nstep; m_robot = hrp::BodyPtr(new hrp::Body()); @@ -147,11 +151,12 @@ RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id) } if(m_angleRefIn.isNew()){ m_angleRefIn.read(); + step = nstep; } - for(int i=0; i 0 ? qold_ref[i] + (m_angleRef.data[i] - qold_ref[i])/step : qold_ref[i]; double dq = (q - qold[i]) / dt; double dq_ref = (q_ref - qold_ref[i]) / dt; qold[i] = q; @@ -159,7 +164,7 @@ RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id) m_torque.data[i] = -(q - q_ref) * Pgain[i] - (dq - dq_ref) * Dgain[i]; double tlimit; if (m_robot && m_robot->numJoints() == dof) { - tlimit = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tlimit_ratio[i]; + tlimit = fabs(m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst * tlimit_ratio[i]); } else { tlimit = (std::numeric_limits::max)() * tlimit_ratio[i]; if (i == 0 && loop % 500 == 0) { @@ -172,6 +177,7 @@ RTC::ReturnCode_t PDcontroller::onExecute(RTC::UniqueId ec_id) } m_torque.data[i] = std::max(std::min(m_torque.data[i], tlimit), -tlimit); } + step--; m_torqueOut.write(); @@ -195,7 +201,7 @@ void PDcontroller::readGainFile() tlimit_ratio.resize(dof); if (gain.is_open()){ double tmp; - for (int i=0; i> tmp) { Pgain[i] = tmp; } else { @@ -238,9 +244,14 @@ void PDcontroller::readGainFile() } } // initialize angleRef, old_ref and old with angle - for(int i=0; i < dof; ++i){ + for(unsigned int i=0; i < dof; ++i){ m_angleRef.data[i] = qold_ref[i] = qold[i] = m_angle.data[i]; } + // Print loaded gain + std::cerr << "[" << m_profile.instance_name << "] loadGain" << std::endl; + for (unsigned int i=0; inumJoints(); i++) { + std::cerr << "[" << m_profile.instance_name << "] " << m_robot->joint(i)->name << ", pgain = " << Pgain[i] << ", dgain = " << Dgain[i] << std::endl; + } } /* diff --git a/rtc/PDcontroller/PDcontroller.h b/rtc/PDcontroller/PDcontroller.h index 8982816bc80..8691d33c1f5 100644 --- a/rtc/PDcontroller/PDcontroller.h +++ b/rtc/PDcontroller/PDcontroller.h @@ -10,6 +10,7 @@ #ifndef PDcontroller_H #define PDcontroller_H +#include #include #include #include @@ -130,13 +131,16 @@ class PDcontroller private: void readGainFile(); hrp::BodyPtr m_robot; - int dummy; - double dt; + double dt; // sampling time of pd control + double ref_dt; // sampling time of renference angles + int step; // current interpolation step + int nstep; // the number of steps to interpolate references std::ifstream gain; std::string gain_fname; hrp::dvector qold, qold_ref, Pgain, Dgain, tlimit_ratio; size_t dof, loop; unsigned int m_debugLevel; + int dummy; }; extern "C" diff --git a/rtc/PDcontroller/PDcontroller.txt b/rtc/PDcontroller/PDcontroller.txt index 80f2439825e..8b6b9b318fc 100644 --- a/rtc/PDcontroller/PDcontroller.txt +++ b/rtc/PDcontroller/PDcontroller.txt @@ -58,7 +58,8 @@ N/A - + + diff --git a/rtc/PlaneRemover/CMakeLists.txt b/rtc/PlaneRemover/CMakeLists.txt index ed53210c466..40e95698512 100644 --- a/rtc/PlaneRemover/CMakeLists.txt +++ b/rtc/PlaneRemover/CMakeLists.txt @@ -14,6 +14,6 @@ target_link_libraries(PlaneRemoverComp ${libs}) set(target PlaneRemover PlaneRemoverComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/PlaneRemover/PlaneRemover.cpp b/rtc/PlaneRemover/PlaneRemover.cpp index aadd292f9e6..471fb58f31d 100644 --- a/rtc/PlaneRemover/PlaneRemover.cpp +++ b/rtc/PlaneRemover/PlaneRemover.cpp @@ -12,7 +12,7 @@ #include #include #include "PlaneRemover.h" -#include "pointcloud.hh" +#include "hrpsys/idl/pointcloud.hh" // Module specification // @@ -148,7 +148,7 @@ RTC::ReturnCode_t PlaneRemover::onExecute(RTC::UniqueId ec_id) pcl::PointCloud::Ptr original (new pcl::PointCloud); original->points.resize(m_original.width*m_original.height); float *src = (float *)m_original.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ original->points[i].x = src[0]; original->points[i].y = src[1]; original->points[i].z = src[2]; @@ -193,7 +193,7 @@ RTC::ReturnCode_t PlaneRemover::onExecute(RTC::UniqueId ec_id) m_filtered.row_step = m_filtered.point_step*m_filtered.width; m_filtered.data.length(m_filtered.height*m_filtered.row_step); float *dst = (float *)m_filtered.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ dst[0] = cloud->points[i].x; dst[1] = cloud->points[i].y; dst[2] = cloud->points[i].z; diff --git a/rtc/PlaneRemover/PlaneRemover.h b/rtc/PlaneRemover/PlaneRemover.h index 6f7896c7bf8..6847115be84 100644 --- a/rtc/PlaneRemover/PlaneRemover.h +++ b/rtc/PlaneRemover/PlaneRemover.h @@ -10,13 +10,14 @@ #ifndef PLANE_REMOVER_H #define PLANE_REMOVER_H +#include +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include "pointcloud.hh" // Service implementation headers // diff --git a/rtc/PointCloudViewer/CMakeLists.txt b/rtc/PointCloudViewer/CMakeLists.txt new file mode 100644 index 00000000000..efa06760402 --- /dev/null +++ b/rtc/PointCloudViewer/CMakeLists.txt @@ -0,0 +1,19 @@ +include_directories(${PCL_INCLUDE_DIRS}) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) + +set(comp_sources PointCloudViewer.cpp) +set(libs hrpsysBaseStub ${PCL_LIBRARIES}) +add_library(PointCloudViewer SHARED ${comp_sources}) +target_link_libraries(PointCloudViewer ${libs}) +set_target_properties(PointCloudViewer PROPERTIES PREFIX "") + +add_executable(PointCloudViewerComp PointCloudViewerComp.cpp ${comp_sources}) +target_link_libraries(PointCloudViewerComp ${libs}) + +set(target PointCloudViewer PointCloudViewerComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/PointCloudViewer/PointCloudViewer.cpp b/rtc/PointCloudViewer/PointCloudViewer.cpp new file mode 100644 index 00000000000..f96ea7e0707 --- /dev/null +++ b/rtc/PointCloudViewer/PointCloudViewer.cpp @@ -0,0 +1,219 @@ +// -*- C++ -*- +/*! + * @file PointCloudViewer.cpp + * @brief Point Cloud Viewer + * $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "PointCloudViewer.h" +#include "hrpsys/idl/pointcloud.hh" +#include + +// Module specification +// +static const char* spec[] = + { + "implementation_id", "PointCloudViewer", + "type_name", "PointCloudViewer", + "description", "Point Cloud Viewer", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + + "" + }; +// + +PointCloudViewer::PointCloudViewer(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_cloudIn("cloud", m_cloud), + // + m_viewer("Point Cloud Viewer"), + dummy(0) +{ +} + +PointCloudViewer::~PointCloudViewer() +{ +} + + + +RTC::ReturnCode_t PointCloudViewer::onInitialize() +{ + //std::cout << m_profile.instance_name << ": onInitialize()" << std::endl; + // + // Bind variables and configuration variable + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("cloudIn", m_cloudIn); + + // Set OutPort buffer + + // Set service provider to Ports + + // Set service consumers to Ports + + // Set CORBA Service Ports + + // + + RTC::Properties& prop = getProperties(); + + return RTC::RTC_OK; +} + + + +/* +RTC::ReturnCode_t PointCloudViewer::onFinalize() +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t PointCloudViewer::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t PointCloudViewer::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t PointCloudViewer::onActivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t PointCloudViewer::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t PointCloudViewer::onExecute(RTC::UniqueId ec_id) +{ + //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + + if (m_cloudIn.isNew()){ + m_cloudIn.read(); + + bool is_color_points = false; + for (int i = 0; i < m_cloud.fields.length(); i++) { + std::string tmp_name(m_cloud.fields[i].name); + if (tmp_name.find("r") != std::string::npos || tmp_name.find("g") != std::string::npos || tmp_name.find("b") != std::string::npos) { + is_color_points = true; // color pointcloud should have rgb field + } + } + + // currently only support PointXYZ and PointXYZRGB + if (is_color_points) { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->is_dense = m_cloud.is_dense; // need to handle pointcloud which has invalid points (is_dense = false) + cloud->points.resize(m_cloud.width*m_cloud.height); + float *src = reinterpret_cast(m_cloud.data.get_buffer()); + for (unsigned int i = 0; i< cloud->points.size(); i++) { + cloud->points[i].x = src[0]; + cloud->points[i].y = src[1]; + cloud->points[i].z = src[2]; + unsigned char *rgb = (unsigned char *)(src+3); + cloud->points[i].r = rgb[0]; + cloud->points[i].g = rgb[1]; + cloud->points[i].b = rgb[2]; + src += 4; + } + if (!m_viewer.wasStopped()){ + m_viewer.showCloud(cloud); + } + } else { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + cloud->points.resize(m_cloud.width*m_cloud.height); + float *src = (float *)m_cloud.data.get_buffer(); + for (unsigned int i=0; ipoints.size(); i++){ + cloud->points[i].x = src[0]; + cloud->points[i].y = src[1]; + cloud->points[i].z = src[2]; + src += 4; + } + if (!m_viewer.wasStopped()){ + m_viewer.showCloud(cloud); + } + } + } + + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t PointCloudViewer::onAborting(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t PointCloudViewer::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t PointCloudViewer::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t PointCloudViewer::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t PointCloudViewer::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + + + +extern "C" +{ + + void PointCloudViewerInit(RTC::Manager* manager) + { + RTC::Properties profile(spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; + + diff --git a/rtc/PointCloudViewer/PointCloudViewer.h b/rtc/PointCloudViewer/PointCloudViewer.h new file mode 100644 index 00000000000..fc488a51f44 --- /dev/null +++ b/rtc/PointCloudViewer/PointCloudViewer.h @@ -0,0 +1,146 @@ +// -*- C++ -*- +/*! + * @file PointCloudViewer.h + * @brief Point Cloud Viewer + * @date $Date$ + * + * $Id$ + */ + +#ifndef POINT_CLOUD_VIEWER_H +#define POINT_CLOUD_VIEWER_H + +#include +#include "hrpsys/idl/pointcloud.hh" +#include +#include +#include +#include +#include +#include +#include + +// Service implementation headers +// + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief sample RT component which has one data input port and one data output port + */ +class PointCloudViewer + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + PointCloudViewer(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~PointCloudViewer(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + // virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + + protected: + // Configuration variable declaration + // + + // + + PointCloudTypes::PointCloud m_cloud; + + // DataInPort declaration + // + InPort m_cloudIn; + + // + + // DataOutPort declaration + // + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + + // + + // Consumer declaration + // + + // + + private: + pcl::visualization::CloudViewer m_viewer; + int dummy; +}; + + +extern "C" +{ + void PointCloudViewerInit(RTC::Manager* manager); +}; + +#endif // POINT_CLOUD_VIEWER_H diff --git a/rtc/PointCloudViewer/PointCloudViewer.txt b/rtc/PointCloudViewer/PointCloudViewer.txt new file mode 100644 index 00000000000..c8e7658faf7 --- /dev/null +++ b/rtc/PointCloudViewer/PointCloudViewer.txt @@ -0,0 +1,51 @@ +/** + +\page PointCloudViewer + +\section introduction Overview + +This component visualizes point clouds + +
    keytypeunitdescription
    dtdouble[s]sampling time
    dtdouble[s]sampling time of PD control
    ref_dtdouble[s]sampling time of reference angles. If this value is specified, reference joint angles are interpolated internally so that they reach the given values after ref_dt. If this value is not specified, the given reference joint angles are used immediately in the PD control.
    modelstd::stringURL of a VRML model
    pdgains_sim_file_namestd::stringFile path for PD gain file.
    pdcontrol_tlimit_ratiovectorRatio vector for torque limit of PDcontroller outputs. If not specified, 1.0 by default.
    + + +
    implementation_idPointCloudViewer
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + +
    port namedata typeunitdescription
    cloudPointCloudTypes::PointCloud
    + +\subsection outports Output Ports + + + +
    port namedata typeunitdescription
    + +\section serviceports Service Ports + +\subsection provider Service Providers + + + +
    port nameinterface nameservice typeIDLdescription
    + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + +
    nametypeunitdefault valuedescription
    + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/PointCloudViewer/PointCloudViewerComp.cpp b/rtc/PointCloudViewer/PointCloudViewerComp.cpp new file mode 100644 index 00000000000..3290eb42155 --- /dev/null +++ b/rtc/PointCloudViewer/PointCloudViewerComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file PointCloudViewerComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "PointCloudViewer.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + PointCloudViewerInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("PointCloudViewer"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/RGB2Gray/CMakeLists.txt b/rtc/RGB2Gray/CMakeLists.txt index ab9685c3276..2238de6569c 100644 --- a/rtc/RGB2Gray/CMakeLists.txt +++ b/rtc/RGB2Gray/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(RGB2GrayComp ${libs}) set(target RGB2Gray RGB2GrayComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/RGB2Gray/RGB2Gray.cpp b/rtc/RGB2Gray/RGB2Gray.cpp index 16e81b78400..d01b1df4ac7 100644 --- a/rtc/RGB2Gray/RGB2Gray.cpp +++ b/rtc/RGB2Gray/RGB2Gray.cpp @@ -7,8 +7,9 @@ * $Id$ */ -#include -#include +#include +#include +#include #include "RGB2Gray.h" // Module specification @@ -122,7 +123,7 @@ RTC::ReturnCode_t RGB2Gray::onExecute(RTC::UniqueId ec_id) cv::Mat src(idat.height, idat.width, CV_8UC3, idat.raw_data.get_buffer()); cv::Mat dst; - cvtColor(src, dst, CV_RGB2GRAY); + cv::cvtColor(src, dst, CV_RGB2GRAY); m_gray.data.image.width = idat.width; m_gray.data.image.height = idat.height; diff --git a/rtc/RGB2Gray/RGB2Gray.h b/rtc/RGB2Gray/RGB2Gray.h index d9c283b7be3..6ae198b23b1 100644 --- a/rtc/RGB2Gray/RGB2Gray.h +++ b/rtc/RGB2Gray/RGB2Gray.h @@ -10,13 +10,14 @@ #ifndef RGB_2_GRAY_H #define RGB_2_GRAY_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/Range2PointCloud/CMakeLists.txt b/rtc/Range2PointCloud/CMakeLists.txt index 330c6190129..a90dc54738f 100644 --- a/rtc/Range2PointCloud/CMakeLists.txt +++ b/rtc/Range2PointCloud/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(Range2PointCloudComp ${libs}) set(target Range2PointCloud Range2PointCloudComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Range2PointCloud/Range2PointCloud.h b/rtc/Range2PointCloud/Range2PointCloud.h index 03790815c50..41a03c63a66 100644 --- a/rtc/Range2PointCloud/Range2PointCloud.h +++ b/rtc/Range2PointCloud/Range2PointCloud.h @@ -10,6 +10,9 @@ #ifndef RANGE2POINTCLOUD_H #define RANGE2POINTCLOUD_H +#include +#include +#include "hrpsys/idl/pointcloud.hh" #include #include #include @@ -17,7 +20,6 @@ #include #include #include -#include "pointcloud.hh" // Service implementation headers // diff --git a/rtc/RangeDataViewer/CMakeLists.txt b/rtc/RangeDataViewer/CMakeLists.txt index 0c4e035f9ae..73fe4838997 100644 --- a/rtc/RangeDataViewer/CMakeLists.txt +++ b/rtc/RangeDataViewer/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(RangeDataViewerComp ${libs}) set(target RangeDataViewer RangeDataViewerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/RangeDataViewer/RangeDataViewer.cpp b/rtc/RangeDataViewer/RangeDataViewer.cpp index e024fd474e2..4d3a637bfaf 100644 --- a/rtc/RangeDataViewer/RangeDataViewer.cpp +++ b/rtc/RangeDataViewer/RangeDataViewer.cpp @@ -9,6 +9,10 @@ #include "RangeDataViewer.h" +#if __cplusplus >= 201103L +using std::isinf; +#endif + // Module specification // static const char* cameraimageviewercomponent_spec[] = diff --git a/rtc/RangeDataViewer/RangeDataViewer.h b/rtc/RangeDataViewer/RangeDataViewer.h index e6362938f0c..fbc25998442 100644 --- a/rtc/RangeDataViewer/RangeDataViewer.h +++ b/rtc/RangeDataViewer/RangeDataViewer.h @@ -10,6 +10,8 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H +#include +#include #include #include #include diff --git a/rtc/RangeNoiseMixer/CMakeLists.txt b/rtc/RangeNoiseMixer/CMakeLists.txt index caf3322c4a8..29d08520212 100644 --- a/rtc/RangeNoiseMixer/CMakeLists.txt +++ b/rtc/RangeNoiseMixer/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(RangeNoiseMixerComp ${libs}) set(target RangeNoiseMixer RangeNoiseMixerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/RangeNoiseMixer/RangeNoiseMixer.h b/rtc/RangeNoiseMixer/RangeNoiseMixer.h index 90b4d1af8a3..e74fec0e8ed 100644 --- a/rtc/RangeNoiseMixer/RangeNoiseMixer.h +++ b/rtc/RangeNoiseMixer/RangeNoiseMixer.h @@ -10,13 +10,14 @@ #ifndef RANGE_NOISE_MIXER_H #define RANGE_NOISE_MIXER_H +#include +#include #include #include #include #include #include #include -#include // Service implementation headers // diff --git a/rtc/ReferenceForceUpdater/CMakeLists.txt b/rtc/ReferenceForceUpdater/CMakeLists.txt new file mode 100644 index 00000000000..f1719c5b8c9 --- /dev/null +++ b/rtc/ReferenceForceUpdater/CMakeLists.txt @@ -0,0 +1,16 @@ +set(comp_sources ReferenceForceUpdater.cpp ReferenceForceUpdaterService_impl.cpp + ../ImpedanceController/JointPathEx.cpp ../ImpedanceController/RatsMatrix.cpp ../SequencePlayer/interpolator.cpp) +set(libs hrpModel-3.1 hrpCollision-3.1 hrpUtil-3.1 hrpsysBaseStub) +add_library(ReferenceForceUpdater SHARED ${comp_sources}) +target_link_libraries(ReferenceForceUpdater ${libs}) +set_target_properties(ReferenceForceUpdater PROPERTIES PREFIX "") + +add_executable(ReferenceForceUpdaterComp ReferenceForceUpdaterComp.cpp ${comp_sources}) +target_link_libraries(ReferenceForceUpdaterComp ${libs}) + +set(target ReferenceForceUpdater ReferenceForceUpdaterComp) + +install(TARGETS ${target} + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib +) diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp new file mode 100644 index 00000000000..86818cbdfa8 --- /dev/null +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.cpp @@ -0,0 +1,882 @@ +// -*- C++ -*- +/*! + * @file ReferenceForceUpdater.cpp + * @brief Update ReferenceForce + * $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include +#include +#include +#include "hrpsys/util/Hrpsys.h" +#include +#include "ReferenceForceUpdater.h" +#include "hrpsys/util/VectorConvert.h" + +typedef coil::Guard Guard; + +// Module specification +// +static const char* ReferenceForceUpdater_spec[] = + { + "implementation_id", "ReferenceForceUpdater", + "type_name", "ReferenceForceUpdater", + "description", "update reference force", + "version", HRPSYS_PACKAGE_VERSION, + "vendor", "AIST", + "category", "example", + "activity_type", "DataFlowComponent", + "max_instance", "10", + "language", "C++", + "lang_type", "compile", + // Configuration variables + "conf.default.debugLevel", "0", + "" + }; +// + +static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm) +{ + int pre = os.precision(); + os.setf(std::ios::fixed); + os << std::setprecision(6) + << (tm.sec + tm.nsec/1e9) + << std::setprecision(pre); + os.unsetf(std::ios::fixed); + return os; +} + +ReferenceForceUpdater::ReferenceForceUpdater(RTC::Manager* manager) + : RTC::DataFlowComponentBase(manager), + // + m_qRefIn("qRef", m_qRef), + m_basePosIn("basePosIn", m_basePos), + m_baseRpyIn("baseRpyIn", m_baseRpy), + m_rpyIn("rpy", m_rpy), + m_diffFootOriginExtMomentIn("diffFootOriginExtMoment", m_diffFootOriginExtMoment), + m_refFootOriginExtMomentOut("refFootOriginExtMoment", m_refFootOriginExtMoment), + m_refFootOriginExtMomentIsHoldValueOut("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValue), + m_ReferenceForceUpdaterServicePort("ReferenceForceUpdaterService"), + // + m_robot(hrp::BodyPtr()), + m_debugLevel(0), + use_sh_base_pos_rpy(false), + footoriginextmoment_name("footoriginextmoment"), + objextmoment0_name("objextmoment0") +{ + m_ReferenceForceUpdaterService.rfu(this); +} + +ReferenceForceUpdater::~ReferenceForceUpdater() +{ +} + + + +RTC::ReturnCode_t ReferenceForceUpdater::onInitialize() +{ + std::cerr << "[" << m_profile.instance_name << "] onInitialize()" << std::endl; + // + // Bind variables and configuration variable + bindParameter("debugLevel", m_debugLevel, "0"); + + // + + // Registration: InPort/OutPort/Service + // + // Set InPort buffers + addInPort("qRef", m_qRefIn); + addInPort("basePosIn", m_basePosIn); + addInPort("baseRpyIn",m_baseRpyIn); + addInPort("rpy",m_rpyIn); + addInPort("diffFootOriginExtMoment", m_diffFootOriginExtMomentIn); + + addOutPort("refFootOriginExtMoment", m_refFootOriginExtMomentOut); + addOutPort("refFootOriginExtMomentIsHoldValue", m_refFootOriginExtMomentIsHoldValueOut); + + // Set service provider to Ports + m_ReferenceForceUpdaterServicePort.registerProvider("service0", "ReferenceForceUpdaterService", m_ReferenceForceUpdaterService); + + // Set service consumers to Ports + // Set CORBA Service Ports + addPort(m_ReferenceForceUpdaterServicePort); + + // Get dt + RTC::Properties& prop = getProperties(); // get properties information from .wrl file + coil::stringTo(m_dt, prop["dt"].c_str()); + + // Make m_robot instance + m_robot = hrp::BodyPtr(new hrp::Body()); + RTC::Manager& rtcManager = RTC::Manager::instance(); + std::string nameServer = rtcManager.getConfig()["corba.nameservers"]; + int comPos = nameServer.find(","); + if (comPos < 0){ + comPos = nameServer.length(); + } + nameServer = nameServer.substr(0, comPos); + RTC::CorbaNaming naming(rtcManager.getORB(), nameServer.c_str()); + if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), // load robot model for m_robot + CosNaming::NamingContext::_duplicate(naming.getRootContext()) + )){ + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl; + return RTC::RTC_ERROR; + } + + // Setting for wrench data ports (real + virtual) + std::vector fsensor_names; + // find names for real force sensors + unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE); + for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); + } + // load virtual force sensors + readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name)); + unsigned int nvforce = m_vfs.size(); + for (unsigned int i=0; i::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) { + if (it->second.id == (int)i) fsensor_names.push_back(it->first); + } + } + + // add ports for all force sensors (real force, input and output of ref_force) + unsigned int nforce = npforce + nvforce; + m_force.resize(nforce); + m_forceIn.resize(nforce); + m_ref_force_in.resize(nforce); + m_ref_force_out.resize(nforce); + m_ref_forceIn.resize(nforce); + m_ref_forceOut.resize(nforce); + std::cerr << "[" << m_profile.instance_name << "] create force sensor ports" << std::endl; + for (unsigned int i=0; i(fsensor_names[i].c_str(), m_force[i]); + m_force[i].data.length(6); + registerInPort(fsensor_names[i].c_str(), *m_forceIn[i]); + // ref inport + m_ref_force_in[i].data.length(6); + for (unsigned int j=0; j<6; j++) m_ref_force_in[i].data[j] = 0.0; + m_ref_forceIn[i] = new InPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), m_ref_force_in[i]); + registerInPort(std::string("ref_"+fsensor_names[i]+"In").c_str(), *m_ref_forceIn[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; + // ref Outport + m_ref_force_out[i].data.length(6); + for (unsigned int j=0; j<6; j++) m_ref_force_out[i].data[j] = 0.0; + m_ref_forceOut[i] = new OutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), m_ref_force_out[i]); + registerOutPort(std::string("ref_"+fsensor_names[i]+"Out").c_str(), *m_ref_forceOut[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << fsensor_names[i] << std::endl; + } + + // setting from conf file + // rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle) + coil::vstring end_effectors_str = coil::split(prop["end_effectors"], ","); + if (end_effectors_str.size() > 0) { + size_t prop_num = 10; + size_t num = end_effectors_str.size()/prop_num; + for (size_t i = 0; i < num; i++) { + std::string ee_name, ee_target, ee_base; + coil::stringTo(ee_name, end_effectors_str[i*prop_num].c_str()); + coil::stringTo(ee_target, end_effectors_str[i*prop_num+1].c_str()); + coil::stringTo(ee_base, end_effectors_str[i*prop_num+2].c_str()); + ee_trans eet; + for (size_t j = 0; j < 3; j++) { + coil::stringTo(eet.localPos(j), end_effectors_str[i*prop_num+3+j].c_str()); + } + double tmpv[4]; + for (int j = 0; j < 4; j++ ) { + coil::stringTo(tmpv[j], end_effectors_str[i*prop_num+6+j].c_str()); + } + eet.localR = Eigen::AngleAxis(tmpv[3], hrp::Vector3(tmpv[0], tmpv[1], tmpv[2])).toRotationMatrix(); // rotation in VRML is represented by axis + angle + eet.target_name = ee_target; + { + bool is_ee_exists = false; + for (size_t j = 0; j < nforce; j++) { + hrp::Sensor* sensor = m_robot->sensor(hrp::Sensor::FORCE, j); + hrp::Link* alink = m_robot->link(ee_target); + while (alink != NULL && alink->name != ee_base && !is_ee_exists) { + if ( alink->name == sensor->link->name ) { + is_ee_exists = true; + eet.sensor_name = sensor->name; + } + alink = alink->parent; + } + } + } + ee_map.insert(std::pair(ee_name , eet)); + + if (( ee_name != "rleg" ) && ( ee_name != "lleg" )) + m_RFUParam.insert(std::pair(ee_name , ReferenceForceUpdaterParam(m_dt))); + + ee_index_map.insert(std::pair(ee_name, i)); + ref_force.push_back(hrp::Vector3::Zero()); + //ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt))); + ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt, interpolator::LINEAR))); + if (( ee_name != "lleg" ) && ( ee_name != "rleg" )) transition_interpolator.insert(std::pair(ee_name, new interpolator(1, m_dt))); + std::cerr << "[" << m_profile.instance_name << "] End Effector [" << ee_name << "]" << ee_target << " " << ee_base << std::endl; + std::cerr << "[" << m_profile.instance_name << "] target = " << ee_target << ", base = " << ee_base << ", sensor_name = " << eet.sensor_name << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localPos = " << eet.localPos.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localR = " << eet.localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; + } + // For FootOriginExtMoment + { + std::string ee_name = footoriginextmoment_name; + m_RFUParam.insert(std::pair(ee_name, ReferenceForceUpdaterParam(m_dt))); + // Initial param + m_RFUParam[ee_name].update_freq = 1/m_dt; // [Hz], update in every control loop + m_RFUParam[ee_name].update_count = 1; // update in every control loop, round((1/rfu_param.update_freq)/m_dt) + m_RFUParam[ee_name].update_time_ratio = 1.0; + m_RFUParam[ee_name].p_gain = 0.003; + m_RFUParam[ee_name].act_force_filter->setCutOffFreq(25.0); // [Hz] + ee_trans eet; + eet.localPos = hrp::Vector3::Zero(); + eet.localR = hrp::Matrix33::Identity(); + eet.target_name = ""; + ee_map.insert(std::pair(ee_name , eet)); + ee_index_map.insert(std::pair(ee_name, ref_force.size())); + ref_force.push_back(hrp::Vector3::Zero()); + ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt, interpolator::LINEAR))); + transition_interpolator.insert(std::pair(ee_name, new interpolator(1, m_dt))); + } + // For ObjExtMoment0 + { + std::string ee_name = objextmoment0_name; + m_RFUParam.insert(std::pair(ee_name, ReferenceForceUpdaterParam(m_dt))); + // Initial param + m_RFUParam[ee_name].update_freq = 1/m_dt; // [Hz], update in every control loop + m_RFUParam[ee_name].update_count = 1; // update in every control loop, round((1/rfu_param.update_freq)/m_dt) + m_RFUParam[ee_name].update_time_ratio = 1.0; + m_RFUParam[ee_name].act_force_filter->setCutOffFreq(20.0); // [Hz] + // other param + ee_trans eet; + eet.localPos = hrp::Vector3::Zero(); + eet.localR = hrp::Matrix33::Identity(); + eet.target_name = ""; + ee_map.insert(std::pair(ee_name , eet)); + ee_index_map.insert(std::pair(ee_name, ref_force.size())); + ref_force.push_back(hrp::Vector3::Zero()); + ref_force_interpolator.insert(std::pair(ee_name, new interpolator(3, m_dt, interpolator::LINEAR))); + transition_interpolator.insert(std::pair(ee_name, new interpolator(1, m_dt))); + } + } + + // check if the dof of m_robot match the number of joint in m_robot + unsigned int dof = m_robot->numJoints(); + for ( unsigned int i = 0 ; i < dof; i++ ){ + if ( (int)i != m_robot->joint(i)->jointId ) { + std::cerr << "[" << m_profile.instance_name << "] jointId is not equal to the index number" << std::endl; + return RTC::RTC_ERROR; + } + } + + loop = 0; + transition_interpolator_ratio.reserve(transition_interpolator.size()); + for (unsigned int i=0; i::iterator it = ref_force_interpolator.begin(); it != ref_force_interpolator.end(); it++ ) { + delete it->second; + } + for ( std::map::iterator it = transition_interpolator.begin(); it != transition_interpolator.end(); it++ ) { + delete it->second; + } + ref_force_interpolator.clear(); + transition_interpolator.clear(); + return RTC::RTC_OK; +} + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onStartup(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onShutdown(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +RTC::ReturnCode_t ReferenceForceUpdater::onActivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name<< "] onActivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + +RTC::ReturnCode_t ReferenceForceUpdater::onDeactivated(RTC::UniqueId ec_id) +{ + std::cerr << "[" << m_profile.instance_name<< "] onDeactivated(" << ec_id << ")" << std::endl; + return RTC::RTC_OK; +} + + +#define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 ) +RTC::ReturnCode_t ReferenceForceUpdater::onExecute(RTC::UniqueId ec_id) +{ + loop ++; + + // check dataport input + for (unsigned int i=0; iisNew() ) { + m_forceIn[i]->read(); + } + if ( m_ref_forceIn[i]->isNew() ) { + m_ref_forceIn[i]->read(); + } + } + if (m_basePosIn.isNew()) { + m_basePosIn.read(); + } + if (m_baseRpyIn.isNew()) { + m_baseRpyIn.read(); + } + if (m_rpyIn.isNew()) { + m_rpyIn.read(); + } + if (m_diffFootOriginExtMomentIn.isNew()) { + m_diffFootOriginExtMomentIn.read(); + } + if (m_qRefIn.isNew()) { + m_qRefIn.read(); + } + + //syncronize m_robot to the real robot + if ( m_qRef.data.length() == m_robot->numJoints() ) { + Guard guard(m_mutex); + + // Interpolator + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + std::string arm = itr->first; + size_t arm_idx = ee_index_map[arm]; + bool transition_interpolator_isEmpty = transition_interpolator[arm]->isEmpty(); + if ( ! transition_interpolator_isEmpty ) { + transition_interpolator[arm]->get(&transition_interpolator_ratio[arm_idx], true); + if ( transition_interpolator[arm]->isEmpty() && m_RFUParam[arm].is_active && m_RFUParam[arm].is_stopping ) { + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm << "] ReferenceForceUpdater [" << arm << "] active => inactive." << std::endl; + m_RFUParam[arm].is_active = false; + m_RFUParam[arm].is_stopping = false; + } + } + } + + // Get and set reference (target) parameters + getTargetParameters(); + + // Get force sensor values + // Force sensor's force value is absolute in reference frame + for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i); + hrp::Vector3 act_force = (sensor->link->R * sensor->localR) * hrp::Vector3(m_force[i].data[0], m_force[i].data[1], m_force[i].data[2]); + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + if (ee_index_map[itr->first] == i) itr->second.act_force_filter->passFilter(act_force); + } + } + // DiffFootOriginExtMoment value is absolute in reference frame + { + hrp::Vector3 df = foot_origin_rot * (-1 * hrp::Vector3(m_diffFootOriginExtMoment.data.x, m_diffFootOriginExtMoment.data.y, m_diffFootOriginExtMoment.data.z)); // diff = ref - act; + m_RFUParam[footoriginextmoment_name].act_force_filter->passFilter(df); + } + + // If RFU is not active + { + bool all_arm_is_not_active = true; + const hrp::Vector3 default_ref_foot_origin_ext_moment = hrp::Vector3::Zero(); + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + std::string arm = itr->first; + size_t arm_idx = ee_index_map[arm]; + if ( m_RFUParam[arm].is_active ) all_arm_is_not_active = false; + else { + if ( arm == footoriginextmoment_name ) { + for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = default_ref_foot_origin_ext_moment(j); + } else if ( arm == objextmoment0_name ) { + for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = 0; + } else { + for (unsigned int j=0; j<3; j++ ) ref_force[arm_idx](j) = m_ref_force_in[arm_idx].data[j]; + } + } + } + //determin ref_force_out from ref_force_in + if ( all_arm_is_not_active ) { + for (unsigned int i=0; iwrite(); + } + m_refFootOriginExtMoment.data.x = default_ref_foot_origin_ext_moment(0); + m_refFootOriginExtMoment.data.y = default_ref_foot_origin_ext_moment(1); + m_refFootOriginExtMoment.data.z = default_ref_foot_origin_ext_moment(2); + m_refFootOriginExtMoment.tm = m_qRef.tm; + m_refFootOriginExtMomentOut.write(); + m_refFootOriginExtMomentIsHoldValue.tm = m_qRef.tm; + m_refFootOriginExtMomentIsHoldValue.data = m_RFUParam[footoriginextmoment_name].is_hold_value; + m_refFootOriginExtMomentIsHoldValueOut.write(); + return RTC::RTC_OK; + } + } + + // If RFU is active + + // Update reference force + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + std::string arm = itr->first; + if ( m_RFUParam[arm].is_active && loop % m_RFUParam[arm].update_count == 0 ) { + if ( arm == footoriginextmoment_name ) updateRefFootOriginExtMoment(arm); + else if ( arm == objextmoment0_name ) updateRefObjExtMoment0(arm); + else updateRefForces(arm); + } + if (!ref_force_interpolator[arm]->isEmpty()) { + ref_force_interpolator[arm]->get(ref_force[ee_index_map[arm]].data(), true); + } + } + } + + //determin ref_force_out from ref_force_in + for (unsigned int i=0; iwrite(); + } + + // FootOriginExtMoment + size_t idx = ee_index_map[footoriginextmoment_name]; + hrp::Vector3 tmp_moment = (foot_origin_rot.transpose() * ref_force[idx]) * transition_interpolator_ratio[idx]; + m_refFootOriginExtMoment.data.x = tmp_moment(0); + m_refFootOriginExtMoment.data.y = tmp_moment(1); + m_refFootOriginExtMoment.data.z = tmp_moment(2); + m_refFootOriginExtMoment.tm = m_qRef.tm; + m_refFootOriginExtMomentOut.write(); + m_refFootOriginExtMomentIsHoldValue.tm = m_qRef.tm; + m_refFootOriginExtMomentIsHoldValue.data = m_RFUParam[footoriginextmoment_name].is_hold_value; + m_refFootOriginExtMomentIsHoldValueOut.write(); + + return RTC::RTC_OK; +} + +void ReferenceForceUpdater::getTargetParameters () +{ + // reference model + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ + m_robot->joint(i)->q = m_qRef.data[i]; + } + m_robot->rootLink()->p = hrp::Vector3(m_basePos.data.x, m_basePos.data.y, m_basePos.data.z); + m_robot->rootLink()->R = hrp::rotFromRpy(m_baseRpy.data.r, m_baseRpy.data.p, m_baseRpy.data.y); + m_robot->calcForwardKinematics(); + if ( (ee_map.find("rleg") != ee_map.end() && ee_map.find("lleg") != ee_map.end()) // if legged robot + && !use_sh_base_pos_rpy ) { + // TODO + // Tempolarily modify root coords to fix foot pos rot + // This will be removed after seq outputs adequate waistRPY discussed in https://github.com/fkanehiro/hrpsys-base/issues/272 + + // get current foot mid pos + rot + std::vector foot_pos; + std::vector foot_rot; + std::vector leg_names; + leg_names.push_back("rleg"); + leg_names.push_back("lleg"); + for (size_t i = 0; i < leg_names.size(); i++) { + hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); + foot_pos.push_back(target_link->p + target_link->R * ee_map[leg_names[i]].localPos); + foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); + } + hrp::Vector3 current_foot_mid_pos ((foot_pos[0]+foot_pos[1])/2.0); + hrp::Matrix33 current_foot_mid_rot; + rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + // calculate fix pos + rot + hrp::Vector3 new_foot_mid_pos(current_foot_mid_pos); + hrp::Matrix33 new_foot_mid_rot; + { + hrp::Vector3 ex = hrp::Vector3::UnitX(); + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 xv1 (current_foot_mid_rot * ex); + xv1(2) = 0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + new_foot_mid_rot(0,0) = xv1(0); new_foot_mid_rot(1,0) = xv1(1); new_foot_mid_rot(2,0) = xv1(2); + new_foot_mid_rot(0,1) = yv1(0); new_foot_mid_rot(1,1) = yv1(1); new_foot_mid_rot(2,1) = yv1(2); + new_foot_mid_rot(0,2) = ez(0); new_foot_mid_rot(1,2) = ez(1); new_foot_mid_rot(2,2) = ez(2); + } + // fix root pos + rot to fix "coords" = "current_foot_mid_xx" + hrp::Matrix33 tmpR (new_foot_mid_rot * current_foot_mid_rot.transpose()); + m_robot->rootLink()->p = new_foot_mid_pos + tmpR * (m_robot->rootLink()->p - current_foot_mid_pos); + rats::rotm3times(m_robot->rootLink()->R, tmpR, m_robot->rootLink()->R); + m_robot->calcForwardKinematics(); + } + + hrp::Vector3 foot_origin_pos; + calcFootOriginCoords(foot_origin_pos, foot_origin_rot); +}; + +void ReferenceForceUpdater::calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot) +{ + rats::coordinates leg_c[2], tmpc; + hrp::Vector3 ez = hrp::Vector3::UnitZ(); + hrp::Vector3 ex = hrp::Vector3::UnitX(); + size_t i = 0; + for (std::map::iterator itr = ee_map.begin(); itr != ee_map.end(); itr++ ) { + if (itr->first.find("leg") == std::string::npos) continue; + hrp::Link* target = m_robot->sensor(itr->second.sensor_name)->link; + leg_c[i].pos = target->p; + hrp::Vector3 xv1(target->R * ex); + xv1(2)=0.0; + xv1.normalize(); + hrp::Vector3 yv1(ez.cross(xv1)); + leg_c[i].rot(0,0) = xv1(0); leg_c[i].rot(1,0) = xv1(1); leg_c[i].rot(2,0) = xv1(2); + leg_c[i].rot(0,1) = yv1(0); leg_c[i].rot(1,1) = yv1(1); leg_c[i].rot(2,1) = yv1(2); + leg_c[i].rot(0,2) = ez(0); leg_c[i].rot(1,2) = ez(1); leg_c[i].rot(2,2) = ez(2); + i++; + } + // Only double support is assumed + rats::mid_coords(tmpc, 0.5, leg_c[0], leg_c[1]); + foot_origin_pos = tmpc.pos; + foot_origin_rot = tmpc.rot; +} + +void ReferenceForceUpdater::updateRefFootOriginExtMoment (const std::string& arm) +{ + double interpolation_time = 0; + size_t arm_idx = ee_index_map[arm]; + hrp::Vector3 df = m_RFUParam[arm].act_force_filter->getCurrentValue(); + if (!m_RFUParam[arm].is_hold_value) + ref_force[arm_idx] = ref_force[arm_idx] + (m_RFUParam[arm].p_gain * transition_interpolator_ratio[arm_idx]) * df; + interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio; + if ( ref_force_interpolator[arm]->isEmpty() ) { + ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); + } + if ( DEBUGP ) { + std::cerr << "[" << m_profile.instance_name << "] Updating reference moment [" << arm << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] diff foot origin ext moment = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm], interpolation_time = " << interpolation_time << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] new foot origin ext moment = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl; + } +}; + +void ReferenceForceUpdater::updateRefObjExtMoment0 (const std::string& arm) +{ + size_t arm_idx = ee_index_map[arm]; + size_t rarm_idx = ee_index_map["rarm"]; + size_t larm_idx = ee_index_map["larm"]; + hrp::Vector3 input_rarm_ref_force = hrp::Vector3(m_ref_force_in[rarm_idx].data[0], m_ref_force_in[rarm_idx].data[1], m_ref_force_in[rarm_idx].data[2]); + hrp::Vector3 input_larm_ref_force = hrp::Vector3(m_ref_force_in[larm_idx].data[0], m_ref_force_in[larm_idx].data[1], m_ref_force_in[larm_idx].data[2]); + hrp::Vector3 current_rarm_ref_force = input_rarm_ref_force + ref_force[arm_idx]; + hrp::Vector3 current_larm_ref_force = input_larm_ref_force - ref_force[arm_idx]; + // + hrp::Vector3 df = hrp::Vector3::Zero(); + hrp::Vector3 diff_rarm_force = (m_RFUParam["rarm"].act_force_filter->getCurrentValue() - current_rarm_ref_force); + if (diff_rarm_force(2) > 0) { // r > a, tarinai + df(2) += diff_rarm_force(2); + } + hrp::Vector3 diff_larm_force = (m_RFUParam["larm"].act_force_filter->getCurrentValue() - current_larm_ref_force); + if (diff_larm_force(2) > 0) { // r > a, tarinai + df(2) -= diff_larm_force(2); + } + if (!m_RFUParam[arm].is_hold_value) { + ref_force[arm_idx] += hrp::Vector3(0,0,((m_RFUParam[arm].p_gain * transition_interpolator_ratio[arm_idx]) * df)(2)); + } + double interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio; + if ( ref_force_interpolator[arm]->isEmpty() ) { + ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); + } + if ( DEBUGP ) { + std::cerr << "[" << m_profile.instance_name << "] Updating reference res moment [" << arm << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] df " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm], interpolation_time = " << interpolation_time << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] buffer = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] diff_rarm_force = " << diff_rarm_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", act_rarm_force = " << m_RFUParam["rarm"].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", ref_rarm_force = " << current_rarm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", input_rarm_ref_force = " << input_rarm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << "[N]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] diff_larm_force = " << diff_larm_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", act_larm_force = " << m_RFUParam["larm"].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", ref_larm_force = " << current_larm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << ", input_larm_ref_force = " << input_larm_ref_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) + << "[N]" << std::endl; + } +}; + +void ReferenceForceUpdater::updateRefForces (const std::string& arm) +{ + hrp::Vector3 internal_force = hrp::Vector3::Zero(); + double interpolation_time = 0; + size_t arm_idx = ee_index_map[arm]; + hrp::Link* target_link = m_robot->link(ee_map[arm].target_name); + hrp::Vector3 abs_motion_dir, df; + hrp::Matrix33 ee_rot; + ee_rot = target_link->R * ee_map[arm].localR; + if ( m_RFUParam[arm].frame=="local" ) + abs_motion_dir = ee_rot * m_RFUParam[arm].motion_dir; + else { + hrp::Matrix33 current_foot_mid_rot; + std::vector foot_rot; + std::vector leg_names; + leg_names.push_back("rleg"); + leg_names.push_back("lleg"); + for (size_t i = 0; i < leg_names.size(); i++) { + hrp::Link* target_link = m_robot->link(ee_map[leg_names[i]].target_name); + foot_rot.push_back(target_link->R * ee_map[leg_names[i]].localR); + } + rats::mid_rot(current_foot_mid_rot, 0.5, foot_rot[0], foot_rot[1]); + abs_motion_dir = current_foot_mid_rot * m_RFUParam[arm].motion_dir; + } + // Calc abs force diff + df = m_RFUParam[arm].act_force_filter->getCurrentValue() - ref_force[arm_idx]; + double inner_product = 0; + if ( ! std::fabs((abs_motion_dir.norm() - 0.0)) < 1e-5 ) { + abs_motion_dir.normalize(); + inner_product = df.dot(abs_motion_dir); + if ( ! (inner_product < 0 && ref_force[arm_idx].dot(abs_motion_dir) < 0.0) ) { + hrp::Vector3 in_f = ee_rot * internal_force; + if (!m_RFUParam[arm].is_hold_value) + ref_force[arm_idx] = ref_force[arm_idx].dot(abs_motion_dir) * abs_motion_dir + in_f + (m_RFUParam[arm].p_gain * inner_product * transition_interpolator_ratio[arm_idx]) * abs_motion_dir; + interpolation_time = (1/m_RFUParam[arm].update_freq) * m_RFUParam[arm].update_time_ratio; + if ( ref_force_interpolator[arm]->isEmpty() ) { + ref_force_interpolator[arm]->setGoal(ref_force[arm_idx].data(), interpolation_time, true); + } + } + } + if ( DEBUGP ) { + std::cerr << "[" << m_profile.instance_name << "] Updating reference force [" << arm << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] inner_product = " << inner_product << "[N], ref_force = " << ref_force[arm_idx].dot(abs_motion_dir) << "[N], interpolation_time = " << interpolation_time << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] new ref_force = " << ref_force[arm_idx].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] filtered act_force = " << m_RFUParam[arm].act_force_filter->getCurrentValue().format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] df = " << df.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[N]" << std::endl; + } +}; + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onAborting(RTC::UniqueId ec_id) +{ +return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onError(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onReset(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onStateUpdate(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +/* +RTC::ReturnCode_t ReferenceForceUpdater::onRateChanged(RTC::UniqueId ec_id) +{ + return RTC::RTC_OK; +} +*/ + +bool ReferenceForceUpdater::setReferenceForceUpdaterParam(const std::string& i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) +{ + std::string arm = std::string(i_name_); + std::cerr << "[" << m_profile.instance_name << "] setReferenceForceUpdaterParam [" << i_name_ << "]" << std::endl; + Guard guard(m_mutex); + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } + if ( std::string(i_param.frame) != "local" && std::string(i_param.frame) != "world" ) { + std::cerr << "[" << m_profile.instance_name << "] \"frame\" parameter must be local/world. could not set \"" << std::string(i_param.frame) << "\"" <setCutOffFreq(i_param.cutoff_freq); + for (size_t i = 0; i < 3; i++ ) m_RFUParam[arm].motion_dir(i) = i_param.motion_dir[i]; + + // Print values + m_RFUParam[arm].printParam(std::string(m_profile.instance_name)); + return true; +}; + +bool ReferenceForceUpdater::getReferenceForceUpdaterParam(const std::string& i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) +{ + std::string arm = std::string(i_name_); + std::cerr << "[" << m_profile.instance_name << "] getReferenceForceUpdaterParam [" << i_name_ << "]" << std::endl; + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } + Guard guard(m_mutex); + i_param->p_gain = m_RFUParam[arm].p_gain; + i_param->d_gain = m_RFUParam[arm].d_gain; + i_param->i_gain = m_RFUParam[arm].i_gain; + i_param->update_freq = m_RFUParam[arm].update_freq; + i_param->update_time_ratio = m_RFUParam[arm].update_time_ratio; + i_param->frame = m_RFUParam[arm].frame.c_str(); + i_param->is_hold_value = m_RFUParam[arm].is_hold_value; + i_param->transition_time = m_RFUParam[arm].transition_time; + i_param->cutoff_freq = m_RFUParam[arm].act_force_filter->getCutOffFreq(); + for (size_t i = 0; i < 3; i++ ) i_param->motion_dir[i] = m_RFUParam[arm].motion_dir(i); + return true; +}; + +bool ReferenceForceUpdater::startReferenceForceUpdaterNoWait(const std::string& i_name_) +{ + std::cerr << "[" << m_profile.instance_name << "] startReferenceForceUpdater [" << i_name_ << "]" << std::endl; + { + Guard guard(m_mutex); + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } + if ( m_RFUParam[i_name_].is_active == true ) + return true; + if (transition_interpolator[i_name_]->isEmpty()) { + m_RFUParam[i_name_].is_active = true; + double tmpstart = 0.0, tmpgoal = 1.0; + size_t arm_idx = ee_index_map[i_name_]; + hrp::Vector3 currentRefForce; + if ( std::string(i_name_) == footoriginextmoment_name ) { + currentRefForce = hrp::Vector3 (m_diffFootOriginExtMoment.data.x, m_diffFootOriginExtMoment.data.y, m_diffFootOriginExtMoment.data.z); + } else if ( std::string(i_name_) == objextmoment0_name ) { + currentRefForce = hrp::Vector3::Zero(); + } else { + currentRefForce = hrp::Vector3( m_ref_force_in[arm_idx].data[0], m_ref_force_in[arm_idx].data[1], m_ref_force_in[arm_idx].data[2] ); + } + ref_force_interpolator[i_name_]->set(currentRefForce.data()); + transition_interpolator[i_name_]->set(&tmpstart); + transition_interpolator[i_name_]->setGoal(&tmpgoal, m_RFUParam[i_name_].transition_time, true); + } else { + return false; + } + } + return true; +}; + +bool ReferenceForceUpdater::stopReferenceForceUpdaterNoWait(const std::string& i_name_) +{ + std::cerr << "[" << m_profile.instance_name << "] stopReferenceForceUpdater [" << i_name_ << "]" << std::endl; + { + Guard guard(m_mutex); + if ( m_RFUParam.find(i_name_) == m_RFUParam.end() ) { + std::cerr << "[" << m_profile.instance_name << "] Could not found reference force updater param [" << i_name_ << "]" << std::endl; + return false; + } + if ( m_RFUParam[i_name_].is_active == false ){//already not active + return true; + } + double tmpstart = 1.0, tmpgoal = 0.0; + transition_interpolator[i_name_]->set(&tmpstart); + transition_interpolator[i_name_]->setGoal(&tmpgoal, m_RFUParam[i_name_].transition_time, true); + m_RFUParam[i_name_].is_stopping = true; + } + return true; +}; + +bool ReferenceForceUpdater::startReferenceForceUpdater(const std::string& i_name_) +{ + bool ret = startReferenceForceUpdaterNoWait(i_name_); + if (ret) waitReferenceForceUpdaterTransition(i_name_); + return ret; +}; + +bool ReferenceForceUpdater::stopReferenceForceUpdater(const std::string& i_name_) +{ + bool ret = stopReferenceForceUpdaterNoWait(i_name_); + if (ret) waitReferenceForceUpdaterTransition(i_name_); + return ret; +}; + +void ReferenceForceUpdater::waitReferenceForceUpdaterTransition(const std::string& i_name_) +{ + while (!transition_interpolator[i_name_]->isEmpty()) usleep(1000); + usleep(1000); +}; + +bool ReferenceForceUpdater::getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names) +{ + std::cerr << "[" << m_profile.instance_name << "] getSupportedReferenceForceUpdaterNameSequence" << std::endl; + Guard guard(m_mutex); + o_names->length(m_RFUParam.size()); + size_t i = 0; + for (std::map::iterator itr = m_RFUParam.begin(); itr != m_RFUParam.end(); itr++ ) { + o_names[i] = itr->first.c_str(); + i++; + } + return true; +}; + +extern "C" +{ + + void ReferenceForceUpdaterInit(RTC::Manager* manager) + { + RTC::Properties profile(ReferenceForceUpdater_spec); + manager->registerFactory(profile, + RTC::Create, + RTC::Delete); + } + +}; diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h new file mode 100644 index 00000000000..9ed17457cf3 --- /dev/null +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.h @@ -0,0 +1,258 @@ +// -*- C++ -*- +/*! + * @file ReferenceForceUpdater.h + * @brief ReferenceForceUpdater + * @date $Date$ + * + * $Id$ + */ + +#ifndef REFERENCEFORCEUPDATOR_COMPONENT_H +#define REFERENCEFORCEUPDATOR_COMPONENT_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "../ImpedanceController/JointPathEx.h" +#include "../ImpedanceController/RatsMatrix.h" +#include "../SequencePlayer/interpolator.h" +#include "../TorqueFilter/IIRFilter.h" +#include + +// #include "ImpedanceOutputGenerator.h" +// #include "ObjectTurnaroundDetector.h" +// Service implementation headers +// +#include "ReferenceForceUpdaterService_impl.h" + +// + +// Service Consumer stub headers +// + +// + +using namespace RTC; + +/** + \brief sample RT component which has one data input port and one data output port + */ +class ReferenceForceUpdater + : public RTC::DataFlowComponentBase +{ + public: + /** + \brief Constructor + \param manager pointer to the Manager + */ + ReferenceForceUpdater(RTC::Manager* manager); + /** + \brief Destructor + */ + virtual ~ReferenceForceUpdater(); + + // The initialize action (on CREATED->ALIVE transition) + // formaer rtc_init_entry() + virtual RTC::ReturnCode_t onInitialize(); + + // The finalize action (on ALIVE->END transition) + // formaer rtc_exiting_entry() + virtual RTC::ReturnCode_t onFinalize(); + + // The startup action when ExecutionContext startup + // former rtc_starting_entry() + // virtual RTC::ReturnCode_t onStartup(RTC::UniqueId ec_id); + + // The shutdown action when ExecutionContext stop + // former rtc_stopping_entry() + // virtual RTC::ReturnCode_t onShutdown(RTC::UniqueId ec_id); + + // The activated action (Active state entry action) + // former rtc_active_entry() + virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id); + + // The deactivated action (Active state exit action) + // former rtc_active_exit() + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + + // The execution action that is invoked periodically + // former rtc_active_do() + virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id); + + // The aborting action when main logic error occurred. + // former rtc_aborting_entry() + // virtual RTC::ReturnCode_t onAborting(RTC::UniqueId ec_id); + + // The error action in ERROR state + // former rtc_error_do() + // virtual RTC::ReturnCode_t onError(RTC::UniqueId ec_id); + + // The reset action that is invoked resetting + // This is same but different the former rtc_init_entry() + // virtual RTC::ReturnCode_t onReset(RTC::UniqueId ec_id); + + // The state update action that is invoked after onExecute() action + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onStateUpdate(RTC::UniqueId ec_id); + + // The action that is invoked when execution context's rate is changed + // no corresponding operation exists in OpenRTm-aist-0.2.0 + // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + + bool setReferenceForceUpdaterParam(const std::string& i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param); + bool getReferenceForceUpdaterParam(const std::string& i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param); + bool startReferenceForceUpdater(const std::string& i_name_); + bool stopReferenceForceUpdater(const std::string& i_name_); + bool startReferenceForceUpdaterNoWait(const std::string& i_name_); + bool stopReferenceForceUpdaterNoWait(const std::string& i_name_); + void waitReferenceForceUpdaterTransition(const std::string& i_name_); + bool getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names); + void getTargetParameters (); + void calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matrix33& foot_origin_rot); + void updateRefFootOriginExtMoment (const std::string& arm); + void updateRefObjExtMoment0 (const std::string& arm); + void updateRefForces (const std::string& arm); + inline bool eps_eq(const double a, const double b, const double eps = 1e-3) { return std::fabs((a)-(b)) <= eps; }; + + protected: + // Configuration variable declaration + // + + // + + // DataInPort declaration + // + TimedDoubleSeq m_qRef; + InPort m_qRefIn; + TimedPoint3D m_basePos; + InPort m_basePosIn; + TimedOrientation3D m_baseRpy; + InPort m_baseRpyIn; + std::vector m_force; + std::vector *> m_forceIn; + std::vector m_ref_force_in; + std::vector *> m_ref_forceIn; + TimedOrientation3D m_rpy; + InPort m_rpyIn; + TimedPoint3D m_diffFootOriginExtMoment; + InPort m_diffFootOriginExtMomentIn; + + // + + // DataOutPort declaration + // + std::vector m_ref_force_out; + std::vector *> m_ref_forceOut; + TimedPoint3D m_refFootOriginExtMoment; + OutPort m_refFootOriginExtMomentOut; + TimedBoolean m_refFootOriginExtMomentIsHoldValue; + OutPort m_refFootOriginExtMomentIsHoldValueOut; + + // + + // CORBA Port declaration + // + + // + + // Service declaration + // + RTC::CorbaPort m_ReferenceForceUpdaterServicePort; + + // + + // Consumer declaration + // + ReferenceForceUpdaterService_impl m_ReferenceForceUpdaterService; + + // + + private: + struct ee_trans { + std::string target_name, sensor_name; + hrp::Vector3 localPos; + hrp::Matrix33 localR; + }; + struct ReferenceForceUpdaterParam { + // Update frequency [Hz] + double update_freq; + // Update time ratio \in [0,1] + double update_time_ratio; + // P gain + double p_gain; + // D gain + double d_gain; + // I gain + double i_gain; + // Transition time[s] + double transition_time; + // Motion direction to update reference force + hrp::Vector3 motion_dir; + std::string frame; + int update_count; + bool is_active, is_stopping, is_hold_value; + boost::shared_ptr > act_force_filter; + void printParam (const std::string print_str) + { + std::cerr << "[" << print_str << "] p_gain = " << p_gain << ", d_gain = " << d_gain << ", i_gain = " << i_gain << std::endl; + std::cerr << "[" << print_str << "] update_freq = " << update_freq << "[Hz], update_count = " << update_count << ", update_time_ratio = " << update_time_ratio << ", transition_time = " << transition_time << "[s], cutoff_freq = " << act_force_filter->getCutOffFreq() << "[Hz]" << std::endl; + std::cerr << "[" << print_str << "] motion_dir = " << motion_dir.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << std::endl; + std::cerr << "[" << print_str << "] frame = " << frame << ", is_hold_value = " << (is_hold_value?"true":"false") << std::endl; + } + void initializeParam () { + //params defined in idl + motion_dir = hrp::Vector3::UnitZ(); + frame="local"; + update_freq = 50; // Hz + update_time_ratio = 0.5; + p_gain = 0.02; + d_gain = 0; + i_gain = 0; + transition_time = 1.0; + //additional params (not defined in idl) + is_active = false; + is_stopping = false; + is_hold_value = false; + }; + ReferenceForceUpdaterParam () { + initializeParam(); + }; + ReferenceForceUpdaterParam (const double _dt) { + initializeParam(); + update_count = round((1/update_freq)/_dt); + double default_cutoff_freq = 1e8; + act_force_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(default_cutoff_freq, _dt, hrp::Vector3::Zero())); + }; + }; + std::map m_vfs; + hrp::BodyPtr m_robot; + double m_dt; + unsigned int m_debugLevel; + coil::Mutex m_mutex; + std::map ee_map; + std::map ee_index_map; + std::map m_RFUParam; + std::vector ref_force; + std::map ref_force_interpolator; + std::map transition_interpolator; + std::vector transition_interpolator_ratio; + hrp::Matrix33 foot_origin_rot; + bool use_sh_base_pos_rpy; + int loop;//counter in onExecute + const std::string footoriginextmoment_name, objextmoment0_name; +}; + + +extern "C" +{ + void ReferenceForceUpdaterInit(RTC::Manager* manager); +}; + +#endif // REFERENCEFORCEUPDATOR_COMPONENT_H diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdater.txt b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.txt new file mode 100644 index 00000000000..daf4e3035d3 --- /dev/null +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdater.txt @@ -0,0 +1,74 @@ +/** + +\page ReferenceForceUpdater + +\section introduction Overview + +This component ,ReferenceForceUpdater(rfu), helps biped robot keep balance in accordance with the forces applied on the arm end effectors. + +ReferenceForceUpdater should be used with AutoBalancer or ImpedanceController, because the output of this component is modified reference force, which is input of AutoBalancer or ImpedanceController. + +ReferenceForceUpdater modifies reference force so that reference force follows actual forces measured by sensors on arms' end effector. + +AutoBalancer and ImpedanceController modifies joint angles, assuming that reference force is applied on end effecters. + +Therefore ReferenceForceUpder is effective when you want to controll biped robot, based on actual force applied on end effecters. + +\subsection mode Mode +This component has two mode:Active and NotActive. + +Active mode: ReferenceForceUpdater get actual force measuerd by sensors and reference force from SequencePlayer or StateHolder, and modifies reference force so that reference force follows actual forces. + +NotActive mode: ReferenceForceUpdater do not modify reference force. It output the same value as input reference force. + + + + + +
    implementation_idReferenceForceUpdater
    categoryexample
    + +\section dataports Data Ports + +\subsection inports Input Ports + + + + + + + + +
    port namedata typeunitdescription
    qRefRTC::TimedDoubleSeq[rad]Reference joint angles
    rpyRTC::TimedOrientation3D[rad]Actual attitude sensor's Roll-Pitch-Yaw angle
    basePosInRTC::TimedPoint3D[m]Input base position
    baseRpyInRTC::TimedOrientation3D[rad]Input base orientation (RPY)
    name of force/torque sensor defined in a VRML model, such as "rhsensor"RTC::TimedDoubleSeq[N],[Nm]raw force/torque in the sensor frame
    + +\subsection outports Output Ports + + + + +
    port namedata typeunitdescription
    "Ref_" + name of force sensor defined in a VRML model + "Out", such as "Ref_rhsensorOut"RTC::TimedDoubleSeq[N]reference force in the sensor frame
    + + +\section serviceports Service Ports + +\subsection provider Service Providers + + + + +
    port nameinterface nameservice typeIDLdescription
    ReferenceForceUpdaterServiceservice0ReferenceForceUpdaterService\ref OpenHRP::ReferenceForceUpdaterService
    + +\subsection consumer Service Consumers + +N/A + +\section configuration Configuration Variables + + + + + +\section conf Configuration File + +N/A + + */ diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterComp.cpp b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterComp.cpp new file mode 100644 index 00000000000..1938ada2f8a --- /dev/null +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterComp.cpp @@ -0,0 +1,91 @@ +// -*- C++ -*- +/*! + * @file ReferenceForceUpdaterComp.cpp + * @brief Standalone component + * @date $Date$ + * + * $Id$ + */ + +#include +#include +#include +#include "ReferenceForceUpdater.h" + + +void MyModuleInit(RTC::Manager* manager) +{ + ReferenceForceUpdaterInit(manager); + RTC::RtcBase* comp; + + // Create a component + comp = manager->createComponent("ReferenceForceUpdater"); + + + // Example + // The following procedure is examples how handle RT-Components. + // These should not be in this function. + + // Get the component's object reference + RTC::RTObject_var rtobj; + rtobj = RTC::RTObject::_narrow(manager->getPOA()->servant_to_reference(comp)); + + // Get the port list of the component + PortServiceList* portlist; + portlist = rtobj->get_ports(); + + // getting port profiles + std::cout << "Number of Ports: "; + std::cout << portlist->length() << std::endl << std::endl; + for (CORBA::ULong i(0), n(portlist->length()); i < n; ++i) + { + PortService_ptr port; + port = (*portlist)[i]; + std::cout << "Port" << i << " (name): "; + std::cout << port->get_port_profile()->name << std::endl; + + RTC::PortInterfaceProfileList iflist; + iflist = port->get_port_profile()->interfaces; + std::cout << "---interfaces---" << std::endl; + for (CORBA::ULong i(0), n(iflist.length()); i < n; ++i) + { + std::cout << "I/F name: "; + std::cout << iflist[i].instance_name << std::endl; + std::cout << "I/F type: "; + std::cout << iflist[i].type_name << std::endl; + const char* pol; + pol = iflist[i].polarity == 0 ? "PROVIDED" : "REQUIRED"; + std::cout << "Polarity: " << pol << std::endl; + } + std::cout << "---properties---" << std::endl; + NVUtil::dump(port->get_port_profile()->properties); + std::cout << "----------------" << std::endl << std::endl; + } + + return; +} + +int main (int argc, char** argv) +{ + RTC::Manager* manager; + manager = RTC::Manager::init(argc, argv); + + // Initialize manager + manager->init(argc, argv); + + // Set module initialization proceduer + // This procedure will be invoked in activateManager() function. + manager->setModuleInitProc(MyModuleInit); + + // Activate manager and register to naming service + manager->activateManager(); + + // run the manager in blocking mode + // runManager(false) is the default. + manager->runManager(); + + // If you want to run the manager in non-blocking mode, do like this + // manager->runManager(true); + + return 0; +} diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp new file mode 100644 index 00000000000..4bdcc8a961b --- /dev/null +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.cpp @@ -0,0 +1,63 @@ +// -*- C++ -*- +#include +#include "ReferenceForceUpdaterService_impl.h" +#include "ReferenceForceUpdater.h" + +ReferenceForceUpdaterService_impl::ReferenceForceUpdaterService_impl() +{ +} + +ReferenceForceUpdaterService_impl::~ReferenceForceUpdaterService_impl() +{ +} + +CORBA::Boolean ReferenceForceUpdaterService_impl::setReferenceForceUpdaterParam(const char *i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param) +{ + return m_rfu->setReferenceForceUpdaterParam(std::string(i_name_), i_param); +}; + +CORBA::Boolean ReferenceForceUpdaterService_impl::getReferenceForceUpdaterParam(const char *i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param) +{ + i_param = new OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam(); + i_param->motion_dir.length(3); + return m_rfu->getReferenceForceUpdaterParam(std::string(i_name_), i_param); +}; + +CORBA::Boolean ReferenceForceUpdaterService_impl::startReferenceForceUpdater(const char *i_name_) +{ + return m_rfu->startReferenceForceUpdater(std::string(i_name_)); +}; + +CORBA::Boolean ReferenceForceUpdaterService_impl::stopReferenceForceUpdater(const char *i_name_) +{ + return m_rfu->stopReferenceForceUpdater(std::string(i_name_)); +}; + +CORBA::Boolean ReferenceForceUpdaterService_impl::startReferenceForceUpdaterNoWait(const char *i_name_) +{ + return m_rfu->startReferenceForceUpdaterNoWait(std::string(i_name_)); +}; + +CORBA::Boolean ReferenceForceUpdaterService_impl::stopReferenceForceUpdaterNoWait(const char *i_name_) +{ + return m_rfu->stopReferenceForceUpdaterNoWait(std::string(i_name_)); +}; + +void ReferenceForceUpdaterService_impl::waitReferenceForceUpdaterTransition(const char* i_name_) +{ + return m_rfu->waitReferenceForceUpdaterTransition(std::string(i_name_)); +}; + + +CORBA::Boolean ReferenceForceUpdaterService_impl::getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names) +{ + o_names = new OpenHRP::ReferenceForceUpdaterService::StrSequence(); + return m_rfu->getSupportedReferenceForceUpdaterNameSequence(o_names); +}; + + +void ReferenceForceUpdaterService_impl::rfu(ReferenceForceUpdater *i_rfu) +{ + m_rfu = i_rfu; +}; + diff --git a/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h new file mode 100644 index 00000000000..eb8e0022ebe --- /dev/null +++ b/rtc/ReferenceForceUpdater/ReferenceForceUpdaterService_impl.h @@ -0,0 +1,40 @@ +// -*- C++ -*- +#ifndef __REFERENCEFORCEUPDATOR_SERVICE_H__ +#define __REFERENCEFORCEUPDATOR_SERVICE_H__ + +#include "hrpsys/idl/ReferenceForceUpdaterService.hh" + +using namespace OpenHRP; + +class ReferenceForceUpdater; + +class ReferenceForceUpdaterService_impl + : public virtual POA_OpenHRP::ReferenceForceUpdaterService, + public virtual PortableServer::RefCountServantBase +{ +public: + /** + \brief constructor + */ + ReferenceForceUpdaterService_impl(); + + /** + \brief destructor + */ + virtual ~ReferenceForceUpdaterService_impl(); + + CORBA::Boolean setReferenceForceUpdaterParam(const char *i_name_, const OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam& i_param); + CORBA::Boolean getReferenceForceUpdaterParam(const char *i_name_, OpenHRP::ReferenceForceUpdaterService::ReferenceForceUpdaterParam_out i_param); + CORBA::Boolean startReferenceForceUpdater(const char *i_name_); + CORBA::Boolean stopReferenceForceUpdater(const char *i_name_); + CORBA::Boolean startReferenceForceUpdaterNoWait(const char *i_name_); + CORBA::Boolean stopReferenceForceUpdaterNoWait(const char *i_name_); + void waitReferenceForceUpdaterTransition(const char* i_name_); + CORBA::Boolean getSupportedReferenceForceUpdaterNameSequence(OpenHRP::ReferenceForceUpdaterService::StrSequence_out o_names); + + void rfu(ReferenceForceUpdater *i_rfu); +private: + ReferenceForceUpdater* m_rfu; +}; + +#endif diff --git a/rtc/RemoveForceSensorLinkOffset/CMakeLists.txt b/rtc/RemoveForceSensorLinkOffset/CMakeLists.txt index 9f71e9b1896..ef3b691cc87 100644 --- a/rtc/RemoveForceSensorLinkOffset/CMakeLists.txt +++ b/rtc/RemoveForceSensorLinkOffset/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(RemoveForceSensorLinkOffsetComp ${libs}) set(target RemoveForceSensorLinkOffset RemoveForceSensorLinkOffsetComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp index 44768857013..2675d2d764a 100644 --- a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp +++ b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.cpp @@ -13,6 +13,8 @@ #include #include +typedef coil::Guard Guard; + // Module specification // static const char* removeforcesensorlinkoffset_spec[] = @@ -40,7 +42,8 @@ RemoveForceSensorLinkOffset::RemoveForceSensorLinkOffset(RTC::Manager* manager) m_rpyIn("rpy", m_rpy), m_RemoveForceSensorLinkOffsetServicePort("RemoveForceSensorLinkOffsetService"), // - m_debugLevel(0) + m_debugLevel(0), + max_sensor_offset_calib_counter(0) { m_service0.rmfsoff(this); } @@ -98,11 +101,11 @@ RTC::ReturnCode_t RemoveForceSensorLinkOffset::onInitialize() return RTC::RTC_ERROR; } - int nforce = m_robot->numSensors(hrp::Sensor::FORCE); + unsigned int nforce = m_robot->numSensors(hrp::Sensor::FORCE); m_force.resize(nforce); m_forceOut.resize(nforce); m_forceIn.resize(nforce); - for (size_t i = 0; i < nforce; i++) { + for (unsigned int i = 0; i < nforce; i++) { hrp::Sensor *s = m_robot->sensor(hrp::Sensor::FORCE, i); m_forceOut[i] = new OutPort(std::string("off_"+s->name).c_str(), m_force[i]); m_forceIn[i] = new InPort(s->name.c_str(), m_force[i]); @@ -111,6 +114,7 @@ RTC::ReturnCode_t RemoveForceSensorLinkOffset::onInitialize() registerOutPort(std::string("off_"+s->name).c_str(), *m_forceOut[i]); m_forcemoment_offset_param.insert(std::pair(s->name, ForceMomentOffsetParam())); } + max_sensor_offset_calib_counter = static_cast(8.0/m_dt); // 8.0[s] by default return RTC::RTC_OK; } @@ -167,12 +171,13 @@ RTC::ReturnCode_t RemoveForceSensorLinkOffset::onExecute(RTC::UniqueId ec_id) } if (m_qCurrentIn.isNew()) { m_qCurrentIn.read(); - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qCurrent.data[i]; } // updateRootLinkPosRot(rpy); m_robot->calcForwardKinematics(); + Guard guard(m_mutex); for (unsigned int i=0; iname(); @@ -187,20 +192,33 @@ RTC::ReturnCode_t RemoveForceSensorLinkOffset::onExecute(RTC::UniqueId ec_id) if ( sensor ) { // real force sensor hrp::Matrix33 sensorR = sensor->link->R * sensor->localR; - hrp::Vector3 mg = hrp::Vector3(0,0, m_forcemoment_offset_param[sensor_name].link_offset_mass * grav * -1); + ForceMomentOffsetParam& fmp = m_forcemoment_offset_param[sensor_name]; + hrp::Vector3 mg = hrp::Vector3(0,0, fmp.link_offset_mass * grav * -1); + hrp::Vector3 cxmg = hrp::Vector3(sensorR * fmp.link_offset_centroid).cross(mg); + // Sensor offset calib + if (fmp.sensor_offset_calib_counter > 0) { // while calibrating + fmp.force_offset_sum += (data_p - sensorR.transpose() * mg); + fmp.moment_offset_sum += (data_r - sensorR.transpose() * cxmg); + fmp.sensor_offset_calib_counter--; + if (fmp.sensor_offset_calib_counter == 0) { + fmp.force_offset = fmp.force_offset_sum/max_sensor_offset_calib_counter; + fmp.moment_offset = fmp.moment_offset_sum/max_sensor_offset_calib_counter; + sem_post(&(fmp.wait_sem)); + } + } // force and moments which do not include offsets - hrp::Vector3 off_force = sensorR * (data_p - m_forcemoment_offset_param[sensor_name].force_offset) - mg; - hrp::Vector3 off_moment = sensorR * (data_r - m_forcemoment_offset_param[sensor_name].moment_offset) - hrp::Vector3(sensorR * m_forcemoment_offset_param[sensor->name].link_offset_centroid).cross(mg); + fmp.off_force = sensorR * (data_p - fmp.force_offset) - mg; + fmp.off_moment = sensorR * (data_r - fmp.moment_offset) - cxmg; // convert absolute force -> sensor local force - off_force = hrp::Vector3(sensorR.transpose() * off_force); - off_moment = hrp::Vector3(sensorR.transpose() * off_moment); + fmp.off_force = hrp::Vector3(sensorR.transpose() * fmp.off_force); + fmp.off_moment = hrp::Vector3(sensorR.transpose() * fmp.off_moment); for (size_t j = 0; j < 3; j++) { - m_force[i].data[j] = off_force(j); - m_force[i].data[3+j] = off_moment(j); + m_force[i].data[j] = fmp.off_force(j); + m_force[i].data[3+j] = fmp.off_moment(j); } if ( DEBUGP ) { - std::cerr << "[" << m_profile.instance_name << "] off force = " << off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl; - std::cerr << "[" << m_profile.instance_name << "] off moment = " << off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] off force = " << fmp.off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] off moment = " << fmp.off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << std::endl; } } else { std::cerr << "[" << m_profile.instance_name << "] unknwon force param " << sensor_name << std::endl; @@ -312,6 +330,91 @@ bool RemoveForceSensorLinkOffset::dumpForceMomentOffsetParams(const std::string& return true; }; +bool RemoveForceSensorLinkOffset::removeForceSensorOffset (const ::OpenHRP::RemoveForceSensorLinkOffsetService::StrSequence& names, const double tm) +{ + std::cerr << "[" << m_profile.instance_name << "] removeForceSensorOffset..." << std::endl; + + // Check argument validity + std::vector valid_names, invalid_names, calibrating_names; + bool is_valid_argument = true; + { + Guard guard(m_mutex); + if ( names.length() == 0 ) { // If no sensor names are specified, calibrate all sensors. + std::cerr << "[" << m_profile.instance_name << "] No sensor names are specified, calibrate all sensors = ["; + for ( std::map::iterator it = m_forcemoment_offset_param.begin(); it != m_forcemoment_offset_param.end(); it++ ) { + valid_names.push_back(it->first); + std::cerr << it->first << " "; + } + std::cerr << "]" << std::endl; + } else { + for (size_t i = 0; i < names.length(); i++) { + std::string name(names[i]); + if ( m_forcemoment_offset_param.find(name) != m_forcemoment_offset_param.end() ) { + if ( m_forcemoment_offset_param[name].sensor_offset_calib_counter == 0 ) { + valid_names.push_back(name); + } else { + calibrating_names.push_back(name); + is_valid_argument = false; + } + } else{ + invalid_names.push_back(name); + is_valid_argument = false; + } + } + } + } + // Return if invalid or calibrating + if ( !is_valid_argument ) { + std::cerr << "[" << m_profile.instance_name << "] Cannot start removeForceSensorOffset, invalid = ["; + for (size_t i = 0; i < invalid_names.size(); i++) std::cerr << invalid_names[i] << " "; + std::cerr << "], calibrating = ["; + for (size_t i = 0; i < calibrating_names.size(); i++) std::cerr << calibrating_names[i] << " "; + std::cerr << "]" << std::endl; + return false; + } + + // Start calibration + // Print output force before calib + std::cerr << "[" << m_profile.instance_name << "] Calibrate sensor names = ["; + for (size_t i = 0; i < valid_names.size(); i++) std::cerr << valid_names[i] << " "; + std::cerr << "]" << std::endl; + { + Guard guard(m_mutex); + for (size_t i = 0; i < valid_names.size(); i++) { + std::cerr << "[" << m_profile.instance_name << "] Offset-removed force before calib [" << valid_names[i] << "], "; + std::cerr << "force = " << m_forcemoment_offset_param[valid_names[i]].off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][N]")) << ", "; + std::cerr << "moment = " << m_forcemoment_offset_param[valid_names[i]].off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][Nm]")); + std::cerr << std::endl; + } + max_sensor_offset_calib_counter = static_cast(tm/m_dt); + for (size_t i = 0; i < valid_names.size(); i++) { + m_forcemoment_offset_param[valid_names[i]].force_offset_sum = hrp::Vector3::Zero(); + m_forcemoment_offset_param[valid_names[i]].moment_offset_sum = hrp::Vector3::Zero(); + m_forcemoment_offset_param[valid_names[i]].sensor_offset_calib_counter = max_sensor_offset_calib_counter; + } + } + // Wait + for (size_t i = 0; i < valid_names.size(); i++) { + sem_wait(&(m_forcemoment_offset_param[valid_names[i]].wait_sem)); + } + // Print output force and offset after calib + { + Guard guard(m_mutex); + std::cerr << "[" << m_profile.instance_name << "] Calibrate done (calib time = " << tm << "[s])" << std::endl; + for (size_t i = 0; i < valid_names.size(); i++) { + std::cerr << "[" << m_profile.instance_name << "] Calibrated offset [" << valid_names[i] << "], "; + std::cerr << "force_offset = " << m_forcemoment_offset_param[valid_names[i]].force_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][N]")) << ", "; + std::cerr << "moment_offset = " << m_forcemoment_offset_param[valid_names[i]].moment_offset.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][Nm]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] Offset-removed force after calib [" << valid_names[i] << "], "; + std::cerr << "force = " << m_forcemoment_offset_param[valid_names[i]].off_force.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][N]")) << ", "; + std::cerr << "moment = " << m_forcemoment_offset_param[valid_names[i]].off_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "][Nm]")); + std::cerr << std::endl; + } + } + std::cerr << "[" << m_profile.instance_name << "] removeForceSensorOffset...done" << std::endl; + return true; +} + /* RTC::ReturnCode_t RemoveForceSensorLinkOffset::onAborting(RTC::UniqueId ec_id) { diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.h b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.h index ce040055e77..f6a431d516b 100644 --- a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.h +++ b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.h @@ -10,6 +10,8 @@ #ifndef REMOVEFORCESENSORLINKOFFSET_H #define REMOVEFORCESENSORLINKOFFSET_H +#include +#include #include #include #include @@ -24,6 +26,7 @@ #include "RemoveForceSensorLinkOffsetService_impl.h" #include "../ImpedanceController/RatsMatrix.h" +#include // Service implementation headers // @@ -105,6 +108,7 @@ class RemoveForceSensorLinkOffset bool getForceMomentOffsetParam(const std::string& i_name_, OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam& i_param_); bool loadForceMomentOffsetParams(const std::string& filename); bool dumpForceMomentOffsetParams(const std::string& filename); + bool removeForceSensorOffset (const ::OpenHRP::RemoveForceSensorLinkOffsetService::StrSequence& names, const double tm); protected: // Configuration variable declaration @@ -156,21 +160,36 @@ class RemoveForceSensorLinkOffset private: struct ForceMomentOffsetParam { hrp::Vector3 force_offset, moment_offset, link_offset_centroid; + hrp::Vector3 off_force, off_moment; double link_offset_mass; + // Members for sensor offset calib + hrp::Vector3 force_offset_sum, moment_offset_sum; + int sensor_offset_calib_counter; + sem_t wait_sem; ForceMomentOffsetParam () : force_offset(hrp::Vector3::Zero()), moment_offset(hrp::Vector3::Zero()), - link_offset_centroid(hrp::Vector3::Zero()), link_offset_mass(0) - {}; + off_force(hrp::Vector3::Zero()), off_moment(hrp::Vector3::Zero()), + link_offset_centroid(hrp::Vector3::Zero()), link_offset_mass(0), + force_offset_sum(hrp::Vector3::Zero()), moment_offset_sum(hrp::Vector3::Zero()), + sensor_offset_calib_counter(0), wait_sem() + { + sem_init(&wait_sem, 0, 0); + }; }; void updateRootLinkPosRot (const hrp::Vector3& rpy); void printForceMomentOffsetParam(const std::string& i_name_); std::map m_forcemoment_offset_param; +#if __cplusplus >= 201103L + constexpr +#endif static const double grav = 9.80665; /* [m/s^2] */ double m_dt; hrp::BodyPtr m_robot; unsigned int m_debugLevel; + int max_sensor_offset_calib_counter; + coil::Mutex m_mutex; }; diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt index 4d41c656051..880b840e696 100644 --- a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt +++ b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffset.txt @@ -22,6 +22,11 @@ Note that "off_xx" values from this component are same as "xx" values from Robot This means, if zero force-moment offset and zero mass offset are specified, "off_xx" values equal to "xx". +\subsection offsettype Offset type +Two types of offsets are considered. +1. Sensor offset (drift, ...etc) : In idl, force_offset and moment_offset +2. Link offset (hands and feets mass properties) : In idl, link_offset_centroid and link_offset_mass + \subsection frame Frame This component requires robot's attitude sensing to know the direction diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.cpp b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.cpp index 34088cb439c..8dd4bc682ec 100644 --- a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.cpp +++ b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.cpp @@ -35,6 +35,11 @@ CORBA::Boolean RemoveForceSensorLinkOffsetService_impl::dumpForceMomentOffsetPar return m_rmfsoff->dumpForceMomentOffsetParams(std::string(filename)); }; +CORBA::Boolean RemoveForceSensorLinkOffsetService_impl::removeForceSensorOffset(const ::OpenHRP::RemoveForceSensorLinkOffsetService::StrSequence& names, CORBA::Double tm) +{ + return m_rmfsoff->removeForceSensorOffset(names, tm); +} + void RemoveForceSensorLinkOffsetService_impl::rmfsoff(RemoveForceSensorLinkOffset *i_rmfsoff) { m_rmfsoff = i_rmfsoff; diff --git a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.h b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.h index 2b73c41d2a0..294d733560a 100644 --- a/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.h +++ b/rtc/RemoveForceSensorLinkOffset/RemoveForceSensorLinkOffsetService_impl.h @@ -2,7 +2,7 @@ #ifndef REMOVEFORCESENSORLINKOFFSETSERVICE_IMPL_H #define REMOVEFORCESENSORLINKOFFSETSERVICE_IMPL_H -#include "RemoveForceSensorLinkOffsetService.hh" +#include "hrpsys/idl/RemoveForceSensorLinkOffsetService.hh" using namespace OpenHRP; @@ -20,6 +20,7 @@ class RemoveForceSensorLinkOffsetService_impl CORBA::Boolean getForceMomentOffsetParam(const char *i_name_, OpenHRP::RemoveForceSensorLinkOffsetService::forcemomentOffsetParam_out i_param_); CORBA::Boolean loadForceMomentOffsetParams(const char *fiename); CORBA::Boolean dumpForceMomentOffsetParams(const char *fiename); + CORBA::Boolean removeForceSensorOffset(const ::OpenHRP::RemoveForceSensorLinkOffsetService::StrSequence& names, CORBA::Double tm); // void rmfsoff(RemoveForceSensorLinkOffset *i_rmfsoff); private: diff --git a/rtc/ResizeImage/CMakeLists.txt b/rtc/ResizeImage/CMakeLists.txt index 820eac16f02..f388f2c0c92 100644 --- a/rtc/ResizeImage/CMakeLists.txt +++ b/rtc/ResizeImage/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(ResizeImageComp ${libs}) set(target ResizeImage ResizeImageComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ResizeImage/ResizeImage.h b/rtc/ResizeImage/ResizeImage.h index 5748a599b8b..1251382aece 100644 --- a/rtc/ResizeImage/ResizeImage.h +++ b/rtc/ResizeImage/ResizeImage.h @@ -10,6 +10,8 @@ #ifndef RESIZE_IMAGE_H #define RESIZE_IMAGE_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include @@ -17,7 +19,6 @@ #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/RobotHardware/CMakeLists.txt b/rtc/RobotHardware/CMakeLists.txt index 1bbe2b7e0c9..9b41f017712 100644 --- a/rtc/RobotHardware/CMakeLists.txt +++ b/rtc/RobotHardware/CMakeLists.txt @@ -12,7 +12,7 @@ target_link_libraries(RobotHardwareComp ${libs}) set(target RobotHardware RobotHardwareComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/RobotHardware/RobotHardware.cpp b/rtc/RobotHardware/RobotHardware.cpp index e67b3d92984..a8f997db565 100644 --- a/rtc/RobotHardware/RobotHardware.cpp +++ b/rtc/RobotHardware/RobotHardware.cpp @@ -8,9 +8,9 @@ */ #include +#include "hrpsys/util/VectorConvert.h" #include "RobotHardware.h" #include "robot.h" -#include "util/VectorConvert.h" #include #include @@ -37,6 +37,7 @@ static const char* robothardware_spec[] = "conf.default.fzLimitRatio", "2.0", "conf.default.servoErrorLimit", ",", "conf.default.jointAccelerationLimit", "0", + "conf.default.servoOnDelay", "0", "" }; @@ -48,13 +49,16 @@ RobotHardware::RobotHardware(RTC::Manager* manager) m_isDemoMode(0), m_qRefIn("qRef", m_qRef), m_dqRefIn("dqRef", m_dqRef), + m_ddqRefIn("ddqRef", m_ddqRef), m_tauRefIn("tauRef", m_tauRef), m_qOut("q", m_q), m_dqOut("dq", m_dq), m_tauOut("tau", m_tau), m_ctauOut("ctau", m_ctau), + m_pdtauOut("pdtau", m_pdtau), m_servoStateOut("servoState", m_servoState), m_emergencySignalOut("emergencySignal", m_emergencySignal), + m_rstate2Out("rstate2", m_rstate2), m_RobotHardwareServicePort("RobotHardwareService"), // dummy(0) @@ -73,14 +77,17 @@ RTC::ReturnCode_t RobotHardware::onInitialize() addInPort("qRef", m_qRefIn); addInPort("dqRef", m_dqRefIn); + addInPort("ddqRef", m_ddqRefIn); addInPort("tauRef", m_tauRefIn); addOutPort("q", m_qOut); addOutPort("dq", m_dqOut); addOutPort("tau", m_tauOut); addOutPort("ctau", m_ctauOut); + addOutPort("pdtau", m_pdtauOut); addOutPort("servoState", m_servoStateOut); addOutPort("emergencySignal", m_emergencySignalOut); + addOutPort("rstate2", m_rstate2Out); // Set service provider to Ports m_RobotHardwareServicePort.registerProvider("service0", "RobotHardwareService", m_service0); @@ -95,6 +102,9 @@ RTC::ReturnCode_t RobotHardware::onInitialize() RTC::Properties& prop = getProperties(); double dt = 0.0; coil::stringTo(dt, prop["dt"].c_str()); + int periodic_rate = 0; + coil::stringTo(periodic_rate, prop["exec_cxt.periodic.rate"].c_str()); + dt = 1.0/periodic_rate; if (!dt) { std::cerr << m_profile.instance_name << ": joint command velocity check is disabled" << std::endl; } @@ -112,8 +122,11 @@ RTC::ReturnCode_t RobotHardware::onInitialize() if (!loadBodyFromModelLoader(m_robot, prop["model"].c_str(), CosNaming::NamingContext::_duplicate(naming.getRootContext()) )){ - std::cerr << "failed to load model[" << prop["model"] << "]" - << std::endl; + if (prop["model"] == ""){ + std::cerr << "[" << m_profile.instance_name << "] path to the model file is not defined. Please check the configuration file" << std::endl; + }else{ + std::cerr << "[" << m_profile.instance_name << "] failed to load model[" << prop["model"] << "]" << std::endl; + } } std::vector keys = prop.propertyNames(); @@ -129,9 +142,11 @@ RTC::ReturnCode_t RobotHardware::onInitialize() m_dq.data.length(m_robot->numJoints()); m_tau.data.length(m_robot->numJoints()); m_ctau.data.length(m_robot->numJoints()); + m_pdtau.data.length(m_robot->numJoints()); m_servoState.data.length(m_robot->numJoints()); m_qRef.data.length(m_robot->numJoints()); m_dqRef.data.length(m_robot->numJoints()); + m_ddqRef.data.length(m_robot->numJoints()); m_tauRef.data.length(m_robot->numJoints()); int ngyro = m_robot->numSensors(Sensor::RATE_GYRO); @@ -174,6 +189,7 @@ RTC::ReturnCode_t RobotHardware::onInitialize() bindParameter("servoErrorLimit", m_robot->m_servoErrorLimit, ","); bindParameter("fzLimitRatio", m_robot->m_fzLimitRatio, "2"); bindParameter("jointAccelerationLimit", m_robot->m_accLimit, "0"); + bindParameter("servoOnDelay", m_robot->m_servoOnDelay, "0"); // @@ -219,16 +235,14 @@ RTC::ReturnCode_t RobotHardware::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) { //std::cout << "RobotHardware:onExecute(" << ec_id << ")" << std::endl; - coil::TimeValue coiltm(coil::gettimeofday()); - Time tm; - tm.sec = coiltm.sec(); - tm.nsec = coiltm.usec() * 1000; + Time tm; + this->getTimeNow(tm); if (!m_isDemoMode){ robot::emg_reason reason; int id; if (m_robot->checkEmergency(reason, id)){ - if (reason == robot::EMG_SERVO_ERROR){ + if (reason == robot::EMG_SERVO_ERROR || reason == robot::EMG_POWER_OFF){ m_robot->servo("all", false); m_emergencySignal.data = reason; m_emergencySignalOut.write(); @@ -258,6 +272,12 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) // output to iob m_robot->writeVelocityCommands(m_dqRef.data.get_buffer()); } + if (m_ddqRefIn.isNew()){ + m_ddqRefIn.read(); + //std::cout << "RobotHardware: dqRef[21] = " << m_dqRef.data[21] << std::endl; + // output to iob + m_robot->writeAccelerationCommands(m_ddqRef.data.get_buffer()); + } if (m_tauRefIn.isNew()){ m_tauRefIn.read(); //std::cout << "RobotHardware: tauRef[21] = " << m_tauRef.data[21] << std::endl; @@ -274,6 +294,8 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) m_tau.tm = tm; m_robot->readJointCommandTorques(m_ctau.data.get_buffer()); m_ctau.tm = tm; + m_robot->readPDControllerTorques(m_pdtau.data.get_buffer()); + m_pdtau.tm = tm; for (unsigned int i=0; ireadGyroSensor(i, rate); @@ -315,6 +337,9 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) m_robot->readExtraServoState(i, (int *)(m_servoState.data[i].get_buffer()+1)); } m_servoState.tm = tm; + + getStatus2(m_rstate2.data); + m_rstate2.tm = tm; m_robot->oneStep(); @@ -322,6 +347,7 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) m_dqOut.write(); m_tauOut.write(); m_ctauOut.write(); + m_pdtauOut.write(); m_servoStateOut.write(); for (unsigned int i=0; iwrite(); @@ -332,10 +358,86 @@ RTC::ReturnCode_t RobotHardware::onExecute(RTC::UniqueId ec_id) for (unsigned int i=0; iwrite(); } + m_rstate2Out.write(); return RTC::RTC_OK; } +template +void getStatus(boost::shared_ptr robot, T& rstate) +{ + rstate.angle.length(robot->numJoints()); + robot->readJointAngles(rstate.angle.get_buffer()); + + rstate.command.length(robot->numJoints()); + robot->readJointCommands(rstate.command.get_buffer()); + + rstate.torque.length(robot->numJoints()); + if (!robot->readJointTorques(rstate.torque.get_buffer())){ + for (unsigned int i=0; inumJoints()); + int v, status; + for(unsigned int i=0; i < rstate.servoState.length(); ++i){ + size_t len = robot->lengthOfExtraServoState(i)+1; + rstate.servoState[i].length(len); + status = 0; + v = robot->readCalibState(i); + status |= v<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT; + v = robot->readPowerState(i); + status |= v<< OpenHRP::RobotHardwareService::POWER_STATE_SHIFT; + v = robot->readServoState(i); + status |= v<< OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; + v = robot->readServoAlarm(i); + status |= v<< OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT; + v = robot->readDriverTemperature(i); + status |= v<< OpenHRP::RobotHardwareService::DRIVER_TEMP_SHIFT; + rstate.servoState[i][0] = status; + robot->readExtraServoState(i, (int *)(rstate.servoState[i].get_buffer()+1)); + } + + rstate.rateGyro.length(robot->numSensors(Sensor::RATE_GYRO)); + for (unsigned int i=0; ireadGyroSensor(i, rstate.rateGyro[i].get_buffer()); + } + + rstate.accel.length(robot->numSensors(Sensor::ACCELERATION)); + for (unsigned int i=0; ireadAccelerometer(i, rstate.accel[i].get_buffer()); + } + + rstate.force.length(robot->numSensors(Sensor::FORCE)); + for (unsigned int i=0; ireadForceSensor(i, rstate.force[i].get_buffer()); + } + + robot->readPowerStatus(rstate.voltage, rstate.current); +} + +void RobotHardware::getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2) +{ + getStatus(m_robot, rstate2); +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 2 + rstate2.batteries.length(m_robot->numBatteries()); + for(unsigned int i=0; ireadBatteryState(i, + rstate2.batteries[i].voltage, + rstate2.batteries[i].current, + rstate2.batteries[i].soc); + } + rstate2.temperature.length(m_robot->numThermometers()); + for (unsigned int i=0; ireadThermometer(i, rstate2.temperature[i]); + } +#endif +} + /* RTC::ReturnCode_t RobotHardware::onAborting(RTC::UniqueId ec_id) { diff --git a/rtc/RobotHardware/RobotHardware.h b/rtc/RobotHardware/RobotHardware.h index 37572557f2c..1450cfd07c8 100644 --- a/rtc/RobotHardware/RobotHardware.h +++ b/rtc/RobotHardware/RobotHardware.h @@ -10,6 +10,10 @@ #ifndef ROBOT_HARDWARE_H #define ROBOT_HARDWARE_H +#include +#include +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include #include #include @@ -17,7 +21,6 @@ #include #include #include -#include "HRPDataTypes.hh" #include @@ -101,6 +104,11 @@ class RobotHardware // no corresponding operation exists in OpenRTm-aist-0.2.0 // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); + virtual inline void getTimeNow(Time &tm) { + coil::TimeValue coiltm(coil::gettimeofday()); + tm.sec = coiltm.sec(); + tm.nsec = coiltm.usec() * 1000; + }; protected: // Configuration variable declaration @@ -122,6 +130,11 @@ class RobotHardware */ TimedDoubleSeq m_dqRef; InPort m_dqRefIn; + /** + \brief array of reference accelerations of joint with jointId + */ + TimedDoubleSeq m_ddqRef; + InPort m_ddqRefIn; /** \brief array of reference torques of joint with jointId */ @@ -146,6 +159,10 @@ class RobotHardware \brief array of commanded torques of joint with jointId */ TimedDoubleSeq m_ctau; + /** + \brief array of PD controller torques of joint with jointId + */ + TimedDoubleSeq m_pdtau; /** \brief vector of actual acceleration (vector length = number of acceleration sensors) */ @@ -161,6 +178,7 @@ class RobotHardware std::vector m_force; OpenHRP::TimedLongSeqSeq m_servoState; TimedLong m_emergencySignal; + OpenHRP::RobotHardwareService::TimedRobotState2 m_rstate2; // DataOutPort declaration // @@ -168,11 +186,13 @@ class RobotHardware OutPort m_dqOut; OutPort m_tauOut; OutPort m_ctauOut; + OutPort m_pdtauOut; std::vector *> m_accOut; std::vector *> m_rateOut; std::vector *> m_forceOut; OutPort m_servoStateOut; OutPort m_emergencySignalOut; + OutPort m_rstate2Out; // @@ -193,7 +213,10 @@ class RobotHardware // + robot *robot_ptr(void) { return m_robot.get(); }; private: + void getStatus2(OpenHRP::RobotHardwareService::RobotState2 &rstate2); + int dummy; boost::shared_ptr m_robot; }; diff --git a/rtc/RobotHardware/RobotHardware.txt b/rtc/RobotHardware/RobotHardware.txt index 1eff532386f..27d84980f63 100644 --- a/rtc/RobotHardware/RobotHardware.txt +++ b/rtc/RobotHardware/RobotHardware.txt @@ -62,6 +62,7 @@ N/A
    nametypeunitdefault valuedescription
    debugLevelint0debug level
    isDemoModeint1:demo mode, 0:otherwise. When the robot is in demo mode, joint servo error limits and force limits are not checked.
    servoErrorLimitstd::vector[rad]joint servo error limits. If any servo error exceeds its limit, all joint servos are turned off. When 0 is set, servo error is not checked
    fzLimitRatiodoubleforce limit. If force in Z direction exceeds this limit, all joint servos are turned off. The limit is given by a ratio to weight of the robot. +
    servoOnDelaydouble[s]delay inserted between servo on commands
    @@ -72,7 +73,31 @@ N/A modelstd::stringURL of a VRML model sensor_id.right_leg_force_sensorunsigned intID of a force/torque sensor whose Z force is checked to detect emergency sensor_id.leg_leg_force_sensorunsigned intID of a force/torque sensor whose Z force is checked to detect emergency +pdgains.file_namestringSettings of controller gain for low level hardware. default value: "Pdgains.sav" +\subsection gainfile How to write a PDgain file + +@par +A line should have maximum 6 numbers. +The first is P gain, the second is I gain, the 3rd is D gain, +the 4th is torque P gain, the 5th is torque I gain, and the 6th is torque D gain. +@par +A line which is blank line and starts with '#' is ignored. +@par +Example of gain file: +@code +# gain settings +# p_gain i_gain d_gain torque p_gain torque i_gain torque d_gain +# arm +100 0 10 +100 0 10 + +10 0 1 +10 0 1 + +1000 100 10 2000 200 20 + +@endcode */ diff --git a/rtc/RobotHardware/RobotHardwareService_impl.cpp b/rtc/RobotHardware/RobotHardwareService_impl.cpp index c2bf882acb5..7359e8f22ae 100644 --- a/rtc/RobotHardware/RobotHardwareService_impl.cpp +++ b/rtc/RobotHardware/RobotHardwareService_impl.cpp @@ -123,7 +123,32 @@ void RobotHardwareService_impl::initializeJointAngle(const char* name, const cha void RobotHardwareService_impl::setServoGainPercentage(const char *jname, double percentage) { - m_robot->setServoGainPercentage(jname, percentage); + m_robot->setServoGainPercentage(jname, percentage, 0); +} + +void RobotHardwareService_impl::setServoPGainPercentage(const char *jname, double percentage) +{ + m_robot->setServoGainPercentage(jname, percentage, 1); +} + +void RobotHardwareService_impl::setServoDGainPercentage(const char *jname, double percentage) +{ + m_robot->setServoGainPercentage(jname, percentage, 2); +} + +void RobotHardwareService_impl::setServoPGainPercentageWithTime(const char *jname, double percentage, double time) +{ + m_robot->setServoGainPercentage(jname, percentage, 1, time); +} + +void RobotHardwareService_impl::setServoDGainPercentageWithTime(const char *jname, double percentage, double time) +{ + m_robot->setServoGainPercentage(jname, percentage, 2, time); +} + +void RobotHardwareService_impl::setServoTorqueGainPercentage(const char *jname, double percentage) +{ + m_robot->setServoTorqueGainPercentage(jname, percentage); } void RobotHardwareService_impl::setServoErrorLimit(const char *jname, double limit) @@ -174,3 +199,56 @@ CORBA::Boolean RobotHardwareService_impl::readDigitalOutput(::OpenHRP::RobotHard dout->length(lengthDigitalOutput()); return m_robot->readDigitalOutput((char *)(dout->get_buffer())); } + +CORBA::Boolean RobotHardwareService_impl::setJointInertia(const char* name, ::CORBA::Double mn) +{ + m_robot->setJointInertia(name, mn); +} + +void RobotHardwareService_impl::setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence& mns) +{ + m_robot->setJointInertias(mns.get_buffer()); +} + + +void RobotHardwareService_impl::enableDisturbanceObserver() +{ + m_robot->enableDisturbanceObserver(); +} + +void RobotHardwareService_impl::disableDisturbanceObserver() +{ + m_robot->disableDisturbanceObserver(); +} + +void RobotHardwareService_impl::setDisturbanceObserverGain(::CORBA::Double gain) +{ + m_robot->setDisturbanceObserverGain(gain); +} + +void RobotHardwareService_impl::setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm) +{ + joint_control_mode mode; + switch(jcm){ + case OpenHRP::RobotHardwareService::FREE: + mode = JCM_FREE; + break; + case OpenHRP::RobotHardwareService::POSITION: + mode = JCM_POSITION; + break; + case OpenHRP::RobotHardwareService::TORQUE: + mode = JCM_TORQUE; + break; + case OpenHRP::RobotHardwareService::VELOCITY: + mode = JCM_VELOCITY; + break; +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + case OpenHRP::RobotHardwareService::POSITION_TORQUE: + mode = JCM_POSITION_TORQUE; + break; +#endif + default: + return; + } + m_robot->setJointControlMode(jname, mode); +} diff --git a/rtc/RobotHardware/RobotHardwareService_impl.h b/rtc/RobotHardware/RobotHardwareService_impl.h index e347c0a17df..c29ccc2c8f8 100644 --- a/rtc/RobotHardware/RobotHardwareService_impl.h +++ b/rtc/RobotHardware/RobotHardwareService_impl.h @@ -4,7 +4,7 @@ #define ROBOTHARDWARESERVICE_IMPL_H #include -#include "RobotHardwareService.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include "robot.h" @@ -22,6 +22,11 @@ class RobotHardwareService_impl CORBA::Boolean power(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss); CORBA::Boolean servo(const char* jname, OpenHRP::RobotHardwareService::SwitchStatus ss); void setServoGainPercentage(const char *jname, double limit); + void setServoPGainPercentage(const char *jname, double limit); + void setServoDGainPercentage(const char *jname, double limit); + void setServoPGainPercentageWithTime(const char *jname, double limit, double time); + void setServoDGainPercentageWithTime(const char *jname, double limit, double time); + void setServoTorqueGainPercentage(const char *jname, double limit); void setServoErrorLimit(const char *jname, double limit); void calibrateInertiaSensor(); void removeForceSensorOffset(); @@ -33,6 +38,12 @@ class RobotHardwareService_impl CORBA::Boolean writeDigitalOutputWithMask(const ::OpenHRP::RobotHardwareService::OctSequence& dout, const ::OpenHRP::RobotHardwareService::OctSequence& mask); CORBA::Long lengthDigitalOutput(); CORBA::Boolean readDigitalOutput(::OpenHRP::RobotHardwareService::OctSequence_out dout); + CORBA::Boolean setJointInertia(const char* name, ::CORBA::Double mn); + void setJointInertias(const ::OpenHRP::RobotHardwareService::DblSequence& mns); + void enableDisturbanceObserver(); + void disableDisturbanceObserver(); + void setDisturbanceObserverGain(::CORBA::Double gain); + void setJointControlMode(const char *jname, OpenHRP::RobotHardwareService::JointControlMode jcm); // void setRobot(boost::shared_ptr& i_robot) { m_robot = i_robot; } private: diff --git a/rtc/RobotHardware/robot.cpp b/rtc/RobotHardware/robot.cpp index 93418e8a02f..66ce2c656d7 100644 --- a/rtc/RobotHardware/robot.cpp +++ b/rtc/RobotHardware/robot.cpp @@ -6,9 +6,9 @@ #include #include #include "defs.h" -#include "io/iob.h" +#include "hrpsys/io/iob.h" #include "robot.h" -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" #define CALIB_COUNT (10*200) #define GAIN_COUNT ( 5*200) @@ -19,8 +19,9 @@ using namespace hrp; -robot::robot(double dt) : m_fzLimitRatio(0), m_maxZmpError(DEFAULT_MAX_ZMP_ERROR), m_calibRequested(false), m_pdgainsFilename("PDgains.sav"), wait_sem(0), m_reportedEmergency(true), m_dt(dt), m_accLimit(0) +robot::robot(double dt) : m_fzLimitRatio(0), m_maxZmpError(DEFAULT_MAX_ZMP_ERROR), m_accLimit(0), m_calibRequested(false), m_pdgainsFilename("PDgains.sav"), m_reportedEmergency(true), m_dt(dt), m_enable_poweroff_check(false),m_servoOnDelay(0) { + sem_init(&wait_sem, 0, 0); m_rLegForceSensorId = m_lLegForceSensorId = -1; } @@ -33,8 +34,10 @@ bool robot::init() { int i; gain_counter.resize(numJoints()); - for (i=0; i> default_pgain[i]; - strm >> dummy; - strm >> default_dgain[i]; + for (int i = 0; i < numJoints(); i++) { + std::string str; + bool getlinep; + while ((getlinep = !!(std::getline(strm, str)))) { + if (str.empty()) { + // std::cerr << "[RobotHardware] : loadGain : skip emptiy line" << std::endl; + continue; + } + if (str[0] == '#') { + // std::cerr << "[RobotHardware] : loadGain : skip # started line" << std::endl; + continue; + } + double tmp; + std::istringstream sstrm(str); + sstrm >> tmp; + default_pgain[i] = tmp; + if(sstrm.eof()) break; + + sstrm >> tmp; // I gain + if(sstrm.eof()) break; + + sstrm >> tmp; + default_dgain[i] = tmp; + if(sstrm.eof()) break; + + sstrm >> tmp; + default_tqpgain[i] = tmp; + if(sstrm.eof()) break; + + sstrm >> tmp; // I gain for torque + if(sstrm.eof()) break; + + sstrm >> tmp; + default_tqdgain[i] = tmp; + + break; + } + if(!getlinep) { + if (i < numJoints()) { + std::cerr << "[RobotHardware] loadGain error: size of gains reading from file (" + << m_pdgainsFilename + << ") does not match size of joints" << std::endl; + } + break; + } } + strm.close(); + // Print loaded gain + std::cerr << "[RobotHardware] loadGain" << std::endl; + for (unsigned int i=0; iname + << ", pgain = " << default_pgain[i] + << ", dgain = " << default_dgain[i] + << ", tqpgain = " << default_tqpgain[i] + << ", tqdgain = " << default_tqdgain[i] + << std::endl; + } return true; } void robot::startInertiaSensorCalibration() { + std::cerr << "[RobotHardware] startInertiaSensorCalibration..." << std::endl; if (numSensors(Sensor::ACCELERATION)==0 && numSensors(Sensor::RATE_GYRO)==0) return; if (isBusy()) return; - for (int j=0; j0) { - for (int j=0; jlink->R * m_sensor->localR; hrp::Vector3 G_rotated = m_sensorR.transpose() * G; @@ -228,7 +297,7 @@ void robot::calibrateInertiaSensorOneStep() } #endif - wait_sem.post(); + sem_post(&wait_sem); } } } @@ -236,7 +305,7 @@ void robot::calibrateInertiaSensorOneStep() void robot::calibrateForceSensorOneStep() { if (force_calib_counter>0) { - for (int j=0; j= 4 + write_torque_pgain(i, new_tqpgain); + write_torque_dgain(i, new_tqdgain); +#endif } } void robot::gain_control() { int i; - for (i=0; i= numJoints()) return false; + if (jid == JID_INVALID || jid >= (int)numJoints()) return false; int com = OFF; @@ -327,13 +403,22 @@ bool robot::servo(int jid, bool turnon) write_dgain(jid, 0); old_pgain[jid] = 0; old_dgain[jid] = 0; +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + write_torque_pgain(jid, 0); + write_torque_dgain(jid, 0); +#endif + old_tqpgain[jid] = 0; + old_tqdgain[jid] = 0; if (turnon){ double angle; read_actual_angle(jid, &angle); write_command_angle(jid, angle); } write_servo(jid, com); - if (turnon) gain_counter[jid] = 0; + if (turnon) { + gain_counter[jid] = 0; + max_gain_count[jid] = GAIN_COUNT; + } return true; } @@ -355,12 +440,12 @@ bool robot::power(const char *jname, bool turnon) bool robot::power(int jid, bool turnon) { - if (jid == JID_INVALID || jid >= numJoints()) return false; + if (jid == JID_INVALID || jid >= (int)numJoints()) return false; int com = OFF; if (turnon) { - for(int i=0; i= 4 + write_torque_pgain(i, 0); + write_torque_dgain(i, 0); +#endif write_servo(i, com); write_power_command(i, com); } } else - for (int i=0; i= 4 + write_torque_pgain(jid, 0); + write_torque_dgain(jid, 0); +#endif write_servo(jid, com); write_power_command(jid, com); } else { @@ -402,8 +498,8 @@ bool robot::isBusy() const if (inertia_calib_counter > 0 || force_calib_counter > 0) return true; - for (int i=0; i= 3 + write_command_accelerations(i_commands); +#endif +} + void robot::readPowerStatus(double &o_voltage, double &o_current) { read_power(&o_voltage, &o_current); @@ -526,11 +629,12 @@ char *time_string() bool robot::checkJointCommands(const double *i_commands) { + return false; // tmp fix if (!m_dt) return false; if (!m_commandOld.size()) return false; int state; - for (int i=0; i= 4 + if (!read_torque_pgain(i, &old_tqpgain[i])) old_tqpgain[i] = tqpgain[i]; + if (!read_torque_dgain(i, &old_tqdgain[i])) old_tqdgain[i] = tqdgain[i]; +#endif + gain_counter[i] = 0; + max_gain_count[i] = max_count; + } + std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for all joints in " << max_count << " loop" << std::endl; + }else if ((l = link(i_jname))){ + if (!read_pgain(l->jointId, &old_pgain[l->jointId])) old_pgain[l->jointId] = pgain[l->jointId]; + if (mode / 2 == 0) pgain[l->jointId] = default_pgain[l->jointId] * i_percentage/100.0;// 0 or 1 + if (!read_dgain(l->jointId, &old_dgain[l->jointId])) old_dgain[l->jointId] = dgain[l->jointId]; + if (mode % 2 == 0) dgain[l->jointId] = default_dgain[l->jointId] * i_percentage/100.0;// 0 or 2 +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + if (!read_torque_pgain(l->jointId, &old_tqpgain[l->jointId])) old_tqpgain[l->jointId] = tqpgain[l->jointId]; + if (!read_torque_dgain(l->jointId, &old_tqdgain[l->jointId])) old_tqdgain[l->jointId] = tqdgain[l->jointId]; +#endif + gain_counter[l->jointId] = 0; + max_gain_count[l->jointId] = max_count; + std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for " << i_jname << " in " << max_count << " loop" << std::endl; + }else{ + char *s = (char *)i_jname; while(*s) {*s=toupper(*s);s++;} + const std::vector jgroup = m_jointGroups[i_jname]; + if (jgroup.size()==0) return false; + for (unsigned int i=0; i= 4 + if (!read_torque_pgain(jgroup[i], &old_tqpgain[jgroup[i]])) old_tqpgain[jgroup[i]] = tqpgain[jgroup[i]]; + if (!read_torque_dgain(jgroup[i], &old_tqdgain[jgroup[i]])) old_tqdgain[jgroup[i]] = tqdgain[jgroup[i]]; +#endif + gain_counter[jgroup[i]] = 0; + max_gain_count[jgroup[i]] = max_count; + } + std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for " << i_jname << " in " << max_count << " loop" << std::endl; + } + return true; +} + +bool robot::setServoTorqueGainPercentage(const char *i_jname, double i_percentage) +{ + if ( i_percentage < 0 && 100 < i_percentage ) { + std::cerr << "[RobotHardware] Invalid percentage " << i_percentage << "[%] for setServoTorqueGainPercentage. Percentage should be in (0, 100)[%]." << std::endl; + return false; + } Link *l = NULL; if (strcmp(i_jname, "all") == 0 || strcmp(i_jname, "ALL") == 0){ for (int i=0; i= 4 + if (!read_torque_pgain(i, &old_tqpgain[i])) old_tqpgain[i] = tqpgain[i]; + tqpgain[i] = default_tqpgain[i] * i_percentage/100.0; + if (!read_torque_dgain(i, &old_tqdgain[i])) old_tqdgain[i] = tqdgain[i]; + tqdgain[i] = default_tqdgain[i] * i_percentage/100.0; +#endif if (!read_pgain(i, &old_pgain[i])) old_pgain[i] = pgain[i]; - pgain[i] = default_pgain[i] * i_percentage/100.0; if (!read_dgain(i, &old_dgain[i])) old_dgain[i] = dgain[i]; - dgain[i] = default_dgain[i] * i_percentage/100.0; gain_counter[i] = 0; } - std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for all joints" << std::endl; + std::cerr << "[RobotHardware] setServoTorqueGainPercentage " << i_percentage << "[%] for all joints" << std::endl; }else if ((l = link(i_jname))){ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 4 + if (!read_torque_pgain(l->jointId, &old_tqpgain[l->jointId])) old_tqpgain[l->jointId] = tqpgain[l->jointId]; + tqpgain[l->jointId] = default_tqpgain[l->jointId] * i_percentage/100.0; + if (!read_torque_dgain(l->jointId, &old_tqdgain[l->jointId])) old_tqdgain[l->jointId] = tqdgain[l->jointId]; + tqdgain[l->jointId] = default_tqdgain[l->jointId] * i_percentage/100.0; +#endif if (!read_pgain(l->jointId, &old_pgain[l->jointId])) old_pgain[l->jointId] = pgain[l->jointId]; - pgain[l->jointId] = default_pgain[l->jointId] * i_percentage/100.0; if (!read_dgain(l->jointId, &old_dgain[l->jointId])) old_dgain[l->jointId] = dgain[l->jointId]; - dgain[l->jointId] = default_dgain[l->jointId] * i_percentage/100.0; gain_counter[l->jointId] = 0; - std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for " << i_jname << std::endl; + std::cerr << "[RobotHardware] setServoTorqueGainPercentage " << i_percentage << "[%] for " << i_jname << std::endl; }else{ char *s = (char *)i_jname; while(*s) {*s=toupper(*s);s++;} const std::vector jgroup = m_jointGroups[i_jname]; if (jgroup.size()==0) return false; for (unsigned int i=0; i= 4 + if (!read_torque_pgain(jgroup[i], &old_tqpgain[jgroup[i]])) old_tqpgain[jgroup[i]] = tqpgain[jgroup[i]]; + tqpgain[jgroup[i]] = default_tqpgain[jgroup[i]] * i_percentage/100.0; + if (!read_torque_dgain(jgroup[i], &old_tqdgain[jgroup[i]])) old_tqdgain[jgroup[i]] = tqdgain[jgroup[i]]; + tqdgain[jgroup[i]] = default_tqdgain[jgroup[i]] * i_percentage/100.0; +#endif if (!read_pgain(jgroup[i], &old_pgain[jgroup[i]])) old_pgain[jgroup[i]] = pgain[jgroup[i]]; - pgain[jgroup[i]] = default_pgain[jgroup[i]] * i_percentage/100.0; if (!read_dgain(jgroup[i], &old_dgain[jgroup[i]])) old_dgain[jgroup[i]] = dgain[jgroup[i]]; - dgain[jgroup[i]] = default_dgain[jgroup[i]] * i_percentage/100.0; gain_counter[jgroup[i]] = 0; } - std::cerr << "[RobotHardware] setServoGainPercentage " << i_percentage << "[%] for " << i_jname << std::endl; + std::cerr << "[RobotHardware] setServoTorqueGainPercentage " << i_percentage << "[%] for " << i_jname << std::endl; } return true; } @@ -660,7 +851,7 @@ bool robot::setServoErrorLimit(const char *i_jname, double i_limit) { Link *l = NULL; if (strcmp(i_jname, "all") == 0 || strcmp(i_jname, "ALL") == 0){ - for (int i=0; i> m_lLegForceSensorId; }else if (key == "pdgains.file_name"){ iss >> m_pdgainsFilename; + }else if (key == "enable_poweroff_check"){ + std::string tmp; + iss >> tmp; + m_enable_poweroff_check = (tmp=="true"); }else{ isKnownKey = false; } @@ -803,3 +998,75 @@ int robot::numThermometers() return 0; #endif } + +bool robot::setJointInertia(const char *jname, double mn) +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + Link *l = link(jname); + if (!l) return false; + int jid = l->jointId; + return write_joint_inertia(jid, mn); +#else + return false; +#endif +} + +void robot::setJointInertias(const double *mns) +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + write_joint_inertias(mns); +#endif +} + +int robot::readPDControllerTorques(double *o_torques) +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + return read_pd_controller_torques(o_torques); +#else + return 0; +#endif +} + +void robot::enableDisturbanceObserver() +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + write_disturbance_observer(ON); +#endif +} + +void robot::disableDisturbanceObserver() +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + write_disturbance_observer(OFF); +#endif +} + +void robot::setDisturbanceObserverGain(double gain) +{ +#if defined(ROBOT_IOB_VERSION) && ROBOT_IOB_VERSION >= 3 + write_disturbance_observer_gain(gain); +#endif +} + +bool robot::setJointControlMode(const char *i_jname, joint_control_mode mode) +{ + Link *l = NULL; + if (strcmp(i_jname, "all") == 0 || strcmp(i_jname, "ALL") == 0) { + for (int i=0; i < numJoints(); i++) { + write_control_mode(i, mode); + } + std::cerr << "[RobotHardware] setJointControlMode for all joints : " << mode << std::endl; + } else if ((l = link(i_jname))) { + write_control_mode(l->jointId, mode); + std::cerr << "[RobotHardware] setJointControlMode for " << i_jname << " : " << mode << std::endl; + } else { + char *s = (char *)i_jname; while(*s) { *s=toupper(*s); s++; } + const std::vector jgroup = m_jointGroups[i_jname]; + if (jgroup.size()==0) return false; + for (unsigned int i=0; i -#include +#include #include +#include "hrpsys/io/iob.h" /** \brief @@ -220,6 +221,19 @@ class robot : public hrp::Body */ void writeVelocityCommands(const double *i_commands); + /** + \brief write array of reference accelerations of joint servo + \param i_commands array of reference accelerations of joint servo[rad/s] + */ + void writeAccelerationCommands(const double *i_commands); + + /** + \brief read array of all pd controller torques[Nm] + \param o_torques array of all pd controller torques + \param TRUE if read successfully, FALSE otherwise + */ + int readPDControllerTorques(double *o_torques); + /** \brief get length of extra servo states \param id joint id @@ -237,7 +251,7 @@ class robot : public hrp::Body /** \brief reasons of emergency */ - typedef enum {EMG_SERVO_ERROR, EMG_FZ, EMG_SERVO_ALARM} emg_reason; + typedef enum {EMG_SERVO_ERROR, EMG_FZ, EMG_SERVO_ALARM, EMG_POWER_OFF} emg_reason; /** \brief check occurrence of emergency state @@ -258,9 +272,19 @@ class robot : public hrp::Body \brief set the parcentage to the default servo gain \param name joint name, part name or "all" \param percentage to joint servo gain[0-100] + \param mode 0 when setting PDgain, 1 when setting Pgain or 2 when setting Dgain + \param transition_time length of time [s] required to change gains \return true if set successfully, false otherwise */ - bool setServoGainPercentage(const char *i_jname, double i_percentage); + bool setServoGainPercentage(const char *i_jname, double i_percentage, int mode, double transition_time=0); + + /** + \brief set the parcentage to the default servo torque gain + \param name joint name, part name or "all" + \param percentage to joint servo gain[0-100] + \return true if set successfully, false otherwise + */ + bool setServoTorqueGainPercentage(const char *i_jname, double i_percentage); /** \brief set servo error limit value for specific joint or joint group @@ -270,12 +294,43 @@ class robot : public hrp::Body */ bool setServoErrorLimit(const char *i_jname, double i_limit); + /** + \brief set joint inertia + \param i_jname joint name + \param i_mn joint inertia + \return true if set successfully, false otherwise + */ + bool setJointInertia(const char *i_jname, double i_mn); + + /** + \brief set joint inertias + \param i_mns array of joint inertia + */ + void setJointInertias(const double *i_mn); + + /** + \brief enable disturbance observer + */ + void enableDisturbanceObserver(); + + /** + \brief disable disturbance observer + */ + void disableDisturbanceObserver(); + + /** + \brief set disturbance observer gain + \param gain disturbance observer gain + */ + void setDisturbanceObserverGain(double gain); + void setProperty(const char *key, const char *value); bool addJointGroup(const char *gname, const std::vector& jnames); std::vector m_servoErrorLimit; double m_fzLimitRatio; double m_maxZmpError; double m_accLimit; + double m_servoOnDelay; bool readDigitalInput(char *o_din); int lengthDigitalInput(); @@ -295,6 +350,14 @@ class robot : public hrp::Body \return the number of thermometers */ int numThermometers(); + + /** + \brief set control mode of joint + \param name joint name, part name or "all" + \param mode control mode name + \return true if set successfully, false otherwise + */ + bool setJointControlMode(const char *i_jname, joint_control_mode mode); private: /** \brief calibrate inertia sensor for one sampling period @@ -320,6 +383,7 @@ class robot : public hrp::Body int inertia_calib_counter, force_calib_counter; std::vector gain_counter; + std::vector max_gain_count; std::vector< boost::array > gyro_sum; std::vector< boost::array > accel_sum; @@ -328,6 +392,8 @@ class robot : public hrp::Body std::vector pgain, old_pgain, default_pgain; std::vector dgain, old_dgain, default_dgain; + std::vector tqpgain, old_tqpgain, default_tqpgain; + std::vector tqdgain, old_tqdgain, default_tqdgain; int m_lLegForceSensorId, m_rLegForceSensorId; std::map > m_jointGroups; @@ -335,10 +401,11 @@ class robot : public hrp::Body std::string m_calibJointName, m_calibOptions; std::string m_pdgainsFilename; bool m_reportedEmergency; - boost::interprocess::interprocess_semaphore wait_sem; + sem_t wait_sem; double m_dt; std::vector m_commandOld, m_velocityOld; hrp::Vector3 G; + bool m_enable_poweroff_check; }; #endif diff --git a/rtc/RotateImage/CMakeLists.txt b/rtc/RotateImage/CMakeLists.txt index bb0f31ebcf3..5f15e058ee6 100644 --- a/rtc/RotateImage/CMakeLists.txt +++ b/rtc/RotateImage/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(RotateImageComp ${libs}) set(target RotateImage RotateImageComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/RotateImage/RotateImage.h b/rtc/RotateImage/RotateImage.h index 5624e16db25..288fae83179 100644 --- a/rtc/RotateImage/RotateImage.h +++ b/rtc/RotateImage/RotateImage.h @@ -10,6 +10,8 @@ #ifndef ROTATE_IMAGE_H #define ROTATE_IMAGE_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include @@ -17,7 +19,6 @@ #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/SORFilter/CMakeLists.txt b/rtc/SORFilter/CMakeLists.txt index 28218dc191f..b539d94d0e7 100644 --- a/rtc/SORFilter/CMakeLists.txt +++ b/rtc/SORFilter/CMakeLists.txt @@ -14,6 +14,6 @@ target_link_libraries(SORFilterComp ${libs}) set(target SORFilter SORFilterComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/SORFilter/SORFilter.cpp b/rtc/SORFilter/SORFilter.cpp index d0f7f1612dd..2ddfa920836 100644 --- a/rtc/SORFilter/SORFilter.cpp +++ b/rtc/SORFilter/SORFilter.cpp @@ -11,7 +11,7 @@ #include #include #include "SORFilter.h" -#include "pointcloud.hh" +#include "hrpsys/idl/pointcloud.hh" // Module specification // @@ -143,12 +143,18 @@ RTC::ReturnCode_t SORFilter::onExecute(RTC::UniqueId ec_id) if (m_originalIn.isNew()){ m_originalIn.read(); + if (!m_original.data.length()){ + m_filtered.width = m_filtered.row_step = 0; + m_filtered.data.length(0); + return RTC::RTC_OK; + } + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); cloud->points.resize(m_original.width*m_original.height); float *src = (float *)m_original.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ cloud->points[i].x = src[0]; cloud->points[i].y = src[1]; cloud->points[i].z = src[2]; @@ -165,7 +171,7 @@ RTC::ReturnCode_t SORFilter::onExecute(RTC::UniqueId ec_id) m_filtered.row_step = m_filtered.point_step*m_filtered.width; m_filtered.data.length(m_filtered.height*m_filtered.row_step); float *dst = (float *)m_filtered.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ dst[0] = cloud_filtered->points[i].x; dst[1] = cloud_filtered->points[i].y; dst[2] = cloud_filtered->points[i].z; diff --git a/rtc/SORFilter/SORFilter.h b/rtc/SORFilter/SORFilter.h index 1889c4b1966..0507df228c5 100644 --- a/rtc/SORFilter/SORFilter.h +++ b/rtc/SORFilter/SORFilter.h @@ -10,13 +10,14 @@ #ifndef STATISTICAL_OUTLIER_REMOVAL_FILTER_H #define STATISTICAL_OUTLIER_REMOVAL_FILTER_H +#include +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include "pointcloud.hh" // Service implementation headers // diff --git a/rtc/SequencePlayer/CMakeLists.txt b/rtc/SequencePlayer/CMakeLists.txt index aeabbe78632..a6b062d04e1 100644 --- a/rtc/SequencePlayer/CMakeLists.txt +++ b/rtc/SequencePlayer/CMakeLists.txt @@ -10,7 +10,7 @@ target_link_libraries(SequencePlayerComp ${libs}) set(target SequencePlayer SequencePlayerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/SequencePlayer/SequencePlayer.cpp b/rtc/SequencePlayer/SequencePlayer.cpp index 4251abcab08..e282db113eb 100644 --- a/rtc/SequencePlayer/SequencePlayer.cpp +++ b/rtc/SequencePlayer/SequencePlayer.cpp @@ -11,7 +11,7 @@ #include #include #include "SequencePlayer.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include #include #include "../ImpedanceController/JointPathEx.h" @@ -34,6 +34,7 @@ static const char* sequenceplayer_spec[] = "lang_type", "compile", // Configuration variables "conf.default.debugLevel", "0", + "conf.default.fixedLink", "", "" }; @@ -47,6 +48,7 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager) m_baseRpyInitIn("baseRpyInit", m_baseRpyInit), m_zmpRefInitIn("zmpRefInit", m_zmpRefInit), m_qRefOut("qRef", m_qRef), + m_qEmergencyOut("qEmergency", m_qEmergency), m_tqRefOut("tqRef", m_tqRef), m_zmpRefOut("zmpRef", m_zmpRef), m_accRefOut("accRef", m_accRef), @@ -55,7 +57,6 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager) m_optionalDataOut("optionalData", m_optionalData), m_SequencePlayerServicePort("SequencePlayerService"), // - m_waitSem(0), m_robot(hrp::BodyPtr()), m_debugLevel(0), m_error_pos(0.0001), @@ -63,6 +64,7 @@ SequencePlayer::SequencePlayer(RTC::Manager* manager) m_iteration(50), dummy(0) { + sem_init(&m_waitSem, 0, 0); m_service0.player(this); m_clearFlag = false; m_waitFlag = false; @@ -86,6 +88,7 @@ RTC::ReturnCode_t SequencePlayer::onInitialize() // Set OutPort buffer addOutPort("qRef", m_qRefOut); + addOutPort("qEmergency", m_qEmergencyOut); addOutPort("tqRef", m_tqRefOut); addOutPort("zmpRef", m_zmpRefOut); addOutPort("accRef", m_accRefOut); @@ -106,6 +109,7 @@ RTC::ReturnCode_t SequencePlayer::onInitialize() // Bind variables and configuration variable bindParameter("debugLevel", m_debugLevel, "0"); + bindParameter("fixedLink", m_fixedLink, ""); // RTC::Properties& prop = getProperties(); @@ -134,18 +138,18 @@ RTC::ReturnCode_t SequencePlayer::onInitialize() // Setting for wrench data ports (real + virtual) std::vector fsensor_names; // find names for real force sensors - int npforce = m_robot->numSensors(hrp::Sensor::FORCE); + unsigned int npforce = m_robot->numSensors(hrp::Sensor::FORCE); for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); } // find names for virtual force sensors coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ","); - int nvforce = virtual_force_sensor.size()/10; + unsigned int nvforce = virtual_force_sensor.size()/10; for (unsigned int i=0; iisEmpty()){ m_clearFlag = false; if (m_waitFlag){ m_waitFlag = false; - m_waitSem.post(); + sem_post(&m_waitSem); } }else{ Guard guard(m_mutex); @@ -251,7 +256,39 @@ RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id) m_zmpRef.data.z = zmp[2]; m_accRef.data.ax = acc[0]; m_accRef.data.ay = acc[1]; - m_accRef.data.az = acc[2]; + m_accRef.data.az = acc[2]; + + if (m_fixedLink != ""){ + for (int i=0; inumJoints(); i++){ + m_robot->joint(i)->q = m_qRef.data[i]; + } + for (int i=0; i<3; i++){ + m_robot->rootLink()->p[i] = pos[i]; + } + m_robot->rootLink()->R = hrp::rotFromRpy(rpy[0], rpy[1], rpy[2]); + m_robot->calcForwardKinematics(); + hrp::Link *root = m_robot->rootLink(); + hrp::Vector3 rootP; + hrp::Matrix33 rootR; + if (m_timeToStartPlaying > 0){ + m_timeToStartPlaying -= dt; + hrp::Link *fixed = m_robot->link(m_fixedLink); + hrp::Matrix33 fixed2rootR = fixed->R.transpose()*root->R; + hrp::Vector3 fixed2rootP = fixed->R.transpose()*(root->p - fixed->p); + rootR = m_fixedR*fixed2rootR; + rootP = m_fixedR*fixed2rootP + m_fixedP; + }else{ + rootR = m_offsetR*m_robot->rootLink()->R; + rootP = m_offsetR*m_robot->rootLink()->p + m_offsetP; + } + hrp::Vector3 rootRpy = hrp::rpyFromRot(rootR); + pos[0] = rootP[0]; + pos[1] = rootP[1]; + pos[2] = rootP[2]; + rpy[0] = rootRpy[0]; + rpy[1] = rootRpy[1]; + rpy[2] = rootRpy[2]; + } m_basePos.data.x = pos[0]; m_basePos.data.y = pos[1]; m_basePos.data.z = pos[2]; @@ -269,6 +306,7 @@ RTC::ReturnCode_t SequencePlayer::onExecute(RTC::UniqueId ec_id) } m_qRef.tm = m_qInit.tm; m_qRefOut.write(); + m_qEmergencyOut.write(); m_tqRefOut.write(); m_zmpRefOut.write(); m_accRefOut.write(); @@ -335,7 +373,7 @@ void SequencePlayer::waitInterpolation() std::cerr << __PRETTY_FUNCTION__ << std::endl; } m_waitFlag = true; - m_waitSem.wait(); + sem_wait(&m_waitSem); } bool SequencePlayer::waitInterpolationOfGroup(const char *gname) @@ -345,7 +383,7 @@ bool SequencePlayer::waitInterpolationOfGroup(const char *gname) } m_gname = gname; m_waitFlag = true; - m_waitSem.wait(); + sem_wait(&m_waitSem); return true; } @@ -360,7 +398,7 @@ bool SequencePlayer::setJointAngle(short id, double angle, double tm) dvector q(m_robot->numJoints()); m_seq->getJointAngles(q.data()); q[id] = angle; - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *j = m_robot->joint(i); if (j) j->q = q[i]; } @@ -381,9 +419,10 @@ bool SequencePlayer::setJointAngles(const double *angles, double tm) } Guard guard(m_mutex); if (!setInitialState()) return false; - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *j = m_robot->joint(i); if (j) j->q = angles[i]; + m_qEmergency.data[i] = angles[i]; } m_robot->calcForwardKinematics(); hrp::Vector3 absZmp = m_robot->calcCM(); @@ -396,6 +435,7 @@ bool SequencePlayer::setJointAngles(const double *angles, double tm) v_tms.push_back(tm); m_seq->setJointAnglesSequence(v_poss, v_tms); m_seq->setZmp(relZmp.data(), tm); + return true; } @@ -410,7 +450,7 @@ bool SequencePlayer::setJointAngles(const double *angles, const bool *mask, if (!setInitialState()) return false; double pose[m_robot->numJoints()]; - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ pose[i] = mask[i] ? angles[i] : m_qInit.data[i]; } m_seq->setJointAngles(pose, tm); @@ -428,15 +468,15 @@ bool SequencePlayer::setJointAnglesSequence(const OpenHRP::dSequenceSequence ang bool tmp_mask[robot()->numJoints()]; if (mask.length() != robot()->numJoints()) { - for (int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = true; + for (unsigned int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = true; }else{ - for (int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = mask.get_buffer()[i]; + for (unsigned int i=0; i < robot()->numJoints(); i++) tmp_mask[i] = mask.get_buffer()[i]; } int len = angless.length(); std::vector v_poss; std::vector v_tms; - for ( int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer()); - for ( int i = 0; i < times.length(); i++ ) v_tms.push_back(times[i]); + for ( unsigned int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer()); + for ( unsigned int i = 0; i < times.length(); i++ ) v_tms.push_back(times[i]); return m_seq->setJointAnglesSequence(v_poss, v_tms); } @@ -464,8 +504,8 @@ bool SequencePlayer::setJointAnglesSequenceOfGroup(const char *gname, const Open std::vector v_poss; std::vector v_tms; - for ( int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer()); - for ( int i = 0; i < times.length(); i++ ) v_tms.push_back(times[i]); + for ( unsigned int i = 0; i < angless.length(); i++ ) v_poss.push_back(angless[i].get_buffer()); + for ( unsigned int i = 0; i < times.length(); i++ ) v_tms.push_back(times[i]); return m_seq->setJointAnglesSequenceOfGroup(gname, v_poss, v_tms, angless.length()>0?angless[0].length():0); } @@ -494,16 +534,16 @@ bool SequencePlayer::setJointAnglesSequenceFull(const OpenHRP::dSequenceSequence int len = i_jvss.length(); std::vector v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals; std::vector v_tms; - for ( int i = 0; i < i_jvss.length(); i++ ) v_jvss.push_back(i_jvss[i].get_buffer()); - for ( int i = 0; i < i_vels.length(); i++ ) v_vels.push_back(i_vels[i].get_buffer()); - for ( int i = 0; i < i_torques.length(); i++ ) v_torques.push_back(i_torques[i].get_buffer()); - for ( int i = 0; i < i_poss.length(); i++ ) v_poss.push_back(i_poss[i].get_buffer()); - for ( int i = 0; i < i_rpys.length(); i++ ) v_rpys.push_back(i_rpys[i].get_buffer()); - for ( int i = 0; i < i_accs.length(); i++ ) v_accs.push_back(i_accs[i].get_buffer()); - for ( int i = 0; i < i_zmps.length(); i++ ) v_zmps.push_back(i_zmps[i].get_buffer()); - for ( int i = 0; i < i_wrenches.length(); i++ ) v_wrenches.push_back(i_wrenches[i].get_buffer()); - for ( int i = 0; i < i_optionals.length(); i++ ) v_optionals.push_back(i_optionals[i].get_buffer()); - for ( int i = 0; i < i_tms.length(); i++ ) v_tms.push_back(i_tms[i]); + for ( unsigned int i = 0; i < i_jvss.length(); i++ ) v_jvss.push_back(i_jvss[i].get_buffer()); + for ( unsigned int i = 0; i < i_vels.length(); i++ ) v_vels.push_back(i_vels[i].get_buffer()); + for ( unsigned int i = 0; i < i_torques.length(); i++ ) v_torques.push_back(i_torques[i].get_buffer()); + for ( unsigned int i = 0; i < i_poss.length(); i++ ) v_poss.push_back(i_poss[i].get_buffer()); + for ( unsigned int i = 0; i < i_rpys.length(); i++ ) v_rpys.push_back(i_rpys[i].get_buffer()); + for ( unsigned int i = 0; i < i_accs.length(); i++ ) v_accs.push_back(i_accs[i].get_buffer()); + for ( unsigned int i = 0; i < i_zmps.length(); i++ ) v_zmps.push_back(i_zmps[i].get_buffer()); + for ( unsigned int i = 0; i < i_wrenches.length(); i++ ) v_wrenches.push_back(i_wrenches[i].get_buffer()); + for ( unsigned int i = 0; i < i_optionals.length(); i++ ) v_optionals.push_back(i_optionals[i].get_buffer()); + for ( unsigned int i = 0; i < i_tms.length(); i++ ) v_tms.push_back(i_tms[i]); return m_seq->setJointAnglesSequenceFull(v_jvss, v_vels, v_torques, v_poss, v_rpys, v_accs, v_zmps, v_wrenches, v_optionals, v_tms); } @@ -573,12 +613,12 @@ bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const d hrp::JointPathExPtr manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_parent_name), m_robot->link(target_name), dt, true, std::string(m_profile.instance_name))); // calc fk - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *j = m_robot->joint(i); if (j) j->q = m_qRef.data.get_buffer()[i]; } m_robot->calcForwardKinematics(); - for ( int i = 0; i < manip->numJoints(); i++ ){ + for ( unsigned int i = 0; i < manip->numJoints(); i++ ){ start_av[i] = manip->joint(i)->q; } @@ -603,9 +643,9 @@ bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const d } manip->setMaxIKError(m_error_pos,m_error_rot); manip->setMaxIKIteration(m_iteration); - std::cerr << "[setTargetPose] Solveing IK with frame" << frame_name << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl; - std::cerr << " Start " << start_p << start_R<< std::endl; - std::cerr << " End " << end_p << end_R<< std::endl; + std::cerr << "[setTargetPose] Solveing IK with frame " << (frame_name? frame_name:"world_frame") << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl; + std::cerr << " Start\n" << start_p << "\n" << start_R<< std::endl; + std::cerr << " End\n" << end_p << "\n" << end_R<< std::endl; // interpolate & calc ik int len = max(((start_p - end_p).norm() / 0.02 ), // 2cm @@ -636,7 +676,7 @@ bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const d return false; } v_pos[i] = (const double *)malloc(sizeof(double)*manip->numJoints()); - for ( int j = 0; j < manip->numJoints(); j++ ){ + for ( unsigned int j = 0; j < manip->numJoints(); j++ ){ ((double *)v_pos[i])[j] = manip->joint(j)->q; } v_tm[i] = tm/len; @@ -653,6 +693,7 @@ bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const d } } + if (!m_seq->resetJointGroup(gname, m_qInit.data.get_buffer())) return false; // reset sequencer bool ret = m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), v_pos.size()>0?indices.size():0); // clean up memory, need to improve @@ -670,6 +711,44 @@ void SequencePlayer::loadPattern(const char *basename, double tm) } Guard guard(m_mutex); if (setInitialState()){ + if (m_fixedLink != ""){ + hrp::Link *l = m_robot->link(m_fixedLink); + if (!l) { + std::cerr << __PRETTY_FUNCTION__ << "can't find a fixed link(" + << m_fixedLink << ")" << std::endl; + m_fixedLink = ""; + return; + } + m_robot->calcForwardKinematics(); // this is not called by setinitialstate() + m_fixedP = l->p; + m_fixedR = l->R; + + std::string pos = std::string(basename)+".pos"; + std::string wst = std::string(basename)+".waist"; + std::ifstream ifspos(pos.c_str()); + std::ifstream ifswst(wst.c_str()); + if (!ifspos.is_open() || !ifswst.is_open()){ + std::cerr << __PRETTY_FUNCTION__ << "can't open " << pos << " or " + << wst << ")" << std::endl; + m_fixedLink = ""; + return; + } + double time; + ifspos >> time; + for (int i=0; inumJoints(); i++){ + ifspos >> m_robot->joint(i)->q; + } + ifswst >> time; + for (int i=0; i<3; i++) ifswst >> m_robot->rootLink()->p[i]; + hrp::Vector3 rpy; + for (int i=0; i<3; i++) ifswst >> rpy[i]; + m_robot->rootLink()->R = hrp::rotFromRpy(rpy); + m_robot->calcForwardKinematics(); + + m_offsetR = m_fixedR*l->R.transpose(); + m_offsetP = m_fixedP - m_offsetR*l->p; + m_timeToStartPlaying = tm; + } m_seq->loadPattern(basename, tm); } } @@ -686,7 +765,7 @@ bool SequencePlayer::setInitialState(double tm) return false; }else{ m_seq->setJointAngles(m_qInit.data.get_buffer(), tm); - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ Link *l = m_robot->joint(i); l->q = m_qInit.data[i]; m_qRef.data[i] = m_qInit.data[i]; // update m_qRef for setTargetPose() @@ -723,10 +802,10 @@ void SequencePlayer::playPattern(const dSequenceSequence& pos, const dSequenceSe std::vector v_pos, v_rpy, v_zmp; std::vector v_tm; - for ( int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer()); - for ( int i = 0; i < rpy.length(); i++ ) v_rpy.push_back(rpy[i].get_buffer()); - for ( int i = 0; i < zmp.length(); i++ ) v_zmp.push_back(zmp[i].get_buffer()); - for ( int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]); + for ( unsigned int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer()); + for ( unsigned int i = 0; i < rpy.length(); i++ ) v_rpy.push_back(rpy[i].get_buffer()); + for ( unsigned int i = 0; i < zmp.length(); i++ ) v_zmp.push_back(zmp[i].get_buffer()); + for ( unsigned int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]); return m_seq->playPattern(v_pos, v_rpy, v_zmp, v_tm, m_qInit.data.get_buffer(), pos.length()>0?pos[0].length():0); } @@ -776,6 +855,7 @@ bool SequencePlayer::removeJointGroup(const char *gname) bool ret; { Guard guard(m_mutex); + if (!setInitialState()) return false; ret = m_seq->removeJointGroup(gname); } return ret; @@ -803,8 +883,8 @@ bool SequencePlayer::playPatternOfGroup(const char *gname, const dSequenceSequen std::vector v_pos; std::vector v_tm; - for ( int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer()); - for ( int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]); + for ( unsigned int i = 0; i < pos.length(); i++ ) v_pos.push_back(pos[i].get_buffer()); + for ( unsigned int i = 0; i < tm.length() ; i++ ) v_tm.push_back(tm[i]); return m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), pos.length()>0?pos[0].length():0); } diff --git a/rtc/SequencePlayer/SequencePlayer.h b/rtc/SequencePlayer/SequencePlayer.h index 9f63978235e..184c3aa893b 100644 --- a/rtc/SequencePlayer/SequencePlayer.h +++ b/rtc/SequencePlayer/SequencePlayer.h @@ -10,7 +10,9 @@ #ifndef SEQUENCEPLAYER_H #define SEQUENCEPLAYER_H -#include +#include +#include +#include #include #include #include @@ -157,6 +159,8 @@ class SequencePlayer std::vector *> m_wrenchesOut; TimedDoubleSeq m_optionalData; OutPort m_optionalDataOut; + TimedDoubleSeq m_qEmergency; + OutPort m_qEmergencyOut; // @@ -181,15 +185,19 @@ class SequencePlayer private: seqplay *m_seq; bool m_clearFlag, m_waitFlag; - boost::interprocess::interprocess_semaphore m_waitSem; + sem_t m_waitSem; hrp::BodyPtr m_robot; std::string m_gname; unsigned int m_debugLevel; - int dummy; size_t optional_data_dim; coil::Mutex m_mutex; double m_error_pos, m_error_rot; short m_iteration; + std::string m_fixedLink; + hrp::Vector3 m_offsetP, m_fixedP; + hrp::Matrix33 m_offsetR, m_fixedR; + double m_timeToStartPlaying; + int dummy; }; diff --git a/rtc/SequencePlayer/SequencePlayerService_impl.cpp b/rtc/SequencePlayer/SequencePlayerService_impl.cpp index 5e27f36dd0d..0ddf366ad93 100644 --- a/rtc/SequencePlayer/SequencePlayerService_impl.cpp +++ b/rtc/SequencePlayer/SequencePlayerService_impl.cpp @@ -253,6 +253,7 @@ void SequencePlayerService_impl::clear() CORBA::Boolean SequencePlayerService_impl::clearOfGroup(const char *gname, CORBA::Double i_limitation) { m_player->player()->clearOfGroup(gname, i_limitation); + return true; } void SequencePlayerService_impl::clearNoWait() diff --git a/rtc/SequencePlayer/SequencePlayerService_impl.h b/rtc/SequencePlayer/SequencePlayerService_impl.h index 5576ba8d54e..03345e4345f 100644 --- a/rtc/SequencePlayer/SequencePlayerService_impl.h +++ b/rtc/SequencePlayer/SequencePlayerService_impl.h @@ -2,7 +2,7 @@ #ifndef SEQPLAYSERVICESVC_IMPL_H #define SEQPLAYSERVICESVC_IMPL_H -#include "SequencePlayerService.hh" +#include "hrpsys/idl/SequencePlayerService.hh" using namespace OpenHRP; diff --git a/rtc/SequencePlayer/interpolator.cpp b/rtc/SequencePlayer/interpolator.cpp index afe0ad08ecd..abfd2e8af89 100644 --- a/rtc/SequencePlayer/interpolator.cpp +++ b/rtc/SequencePlayer/interpolator.cpp @@ -51,9 +51,11 @@ interpolator::~interpolator() void interpolator::clear() { - while (!isEmpty()){ - pop(); - } + q.clear(); + dq.clear(); + ddq.clear(); + length = 0; + remain_t = 0; } // 1dof interpolator @@ -106,7 +108,11 @@ double interpolator::calc_interpolation_time(const double *newg) } remain_t_ = max_diff/default_avg_vel; #define MIN_INTERPOLATION_TIME (1.0) - if (remain_t_ < MIN_INTERPOLATION_TIME) remain_t_ = MIN_INTERPOLATION_TIME; + if (remain_t_ < MIN_INTERPOLATION_TIME) { + std::cerr << "[interpolator][" << name << "] MIN_INTERPOLATION_TIME violated!! Limit remain_t (" << remain_t << ") by MIN_INTERPOLATION_TIME (" << MIN_INTERPOLATION_TIME << ")." + << "(max_diff = " << max_diff << ", default_avg_vel = " << default_avg_vel << ")" << std::endl;; + remain_t_ = MIN_INTERPOLATION_TIME; + } return remain_t_; } @@ -163,7 +169,7 @@ void interpolator::setGoal(const double *newg, const double *newv, double time, a1[i]=v[i]; a2[i]=(-3*x[i] + 3*gx[i] - 2*v[i]*target_t - gv[i]*target_t) / (target_t*target_t); a3[i]=( 2*x[i] - 2*gx[i] + v[i]*target_t + gv[i]*target_t) / (target_t*target_t*target_t); - a4[i]=a[5]=0; + a4[i]=a5[i]=0; break; } } @@ -224,13 +230,13 @@ void interpolator::load(const char *fname, double time_to_start, double scale, vs = new double[dim]; strm >> time; while(strm.eof()==0){ - for (int i=0; i> tmp; } for (int i=0; i> vs[i]; } - for (int i=0; i> tmp; } if (ptime <0){ @@ -389,3 +395,8 @@ double interpolator::remain_time() { return dt*length; } + +double interpolator::get_remain_time() +{ + return remain_t; +} diff --git a/rtc/SequencePlayer/interpolator.h b/rtc/SequencePlayer/interpolator.h index 8703f911936..7412d7d3d49 100644 --- a/rtc/SequencePlayer/interpolator.h +++ b/rtc/SequencePlayer/interpolator.h @@ -48,6 +48,7 @@ class interpolator bool immediate=true, size_t offset1 = 0, size_t offset2 = 0); bool isEmpty(); double remain_time(); + double get_remain_time(); double calc_interpolation_time(const double *g); bool setInterpolationMode (interpolation_mode i_mode_); // Set goal @@ -59,7 +60,7 @@ class interpolator // If remain_t <= 0, do nothing. void interpolate(double& remain_t_); double deltaT() const { return dt; } - double dimension() const { return dim; } + int dimension() const { return dim; } void setName (const std::string& _name) { name = _name; }; private: // Current interpolation mode diff --git a/rtc/SequencePlayer/seqplay.cpp b/rtc/SequencePlayer/seqplay.cpp index acc272bd15e..2ef8e7706ab 100644 --- a/rtc/SequencePlayer/seqplay.cpp +++ b/rtc/SequencePlayer/seqplay.cpp @@ -616,7 +616,7 @@ bool seqplay::setJointAnglesSequence(std::vector pos, std::vector } const double *q_next = pos[i+1]; const double *q_prev = i==0?x:pos[i-1]; - for (unsigned int j = 0; j < m_dof; j++) { + for (int j = 0; j < m_dof; j++) { double d0, d1, v0, v1; d0 = (q[j] - q_prev[j]); d1 = (q_next[j] - q[j]); @@ -629,7 +629,7 @@ bool seqplay::setJointAnglesSequence(std::vector pos, std::vector } } } else { - for (unsigned int j = 0; j < m_dof; j++) { v[j] = 0.0; } + for (int j = 0; j < m_dof; j++) { v[j] = 0.0; } } interpolators[Q]->setGoal(pos[i], v, tm[i], false); @@ -666,7 +666,7 @@ bool seqplay::setJointAnglesSequenceFull(std::vector i_pos, std:: interpolators[Q]->clear(); interpolators[Q]->push(x, v, a, true); double torque[m_dof], dummy_dof[m_dof]; - for (unsigned int j = 0; j < m_dof; j++) { dummy_dof[j] = 0.0; } + for (int j = 0; j < m_dof; j++) { dummy_dof[j] = 0.0; } interpolators[TQ]->get(torque, false); interpolators[TQ]->set(torque); interpolators[TQ]->clear(); @@ -686,8 +686,8 @@ bool seqplay::setJointAnglesSequenceFull(std::vector i_pos, std:: interpolators[ACC]->push(bacc, dummy_3, dummy_3, true); int fnum = interpolators[WRENCHES]->dimension()/6, optional_data_dim = interpolators[OPTIONAL_DATA]->dimension(); double zmp[3], wrench[6*fnum], dummy_fnum[6*fnum], optional[optional_data_dim], dummy_optional[optional_data_dim]; - for (unsigned int j = 0; j < 6*fnum; j++) { dummy_dof[j] = 0.0; } - for (unsigned int j = 0; j < optional_data_dim; j++) { dummy_optional[j] = 0.0; } + for (int j = 0; j < 6*fnum; j++) { dummy_dof[j] = 0.0; } + for (int j = 0; j < optional_data_dim; j++) { dummy_optional[j] = 0.0; } interpolators[ZMP]->get(zmp, false); interpolators[ZMP]->set(zmp); interpolators[ZMP]->clear(); @@ -704,7 +704,7 @@ bool seqplay::setJointAnglesSequenceFull(std::vector i_pos, std:: const double *q=NULL; for (unsigned int i=0; i 0 ) { - for (unsigned int j = 0; j < m_dof; j++) { + for (int j = 0; j < m_dof; j++) { v[j] = i_vel[i][j]; } }else{ @@ -718,7 +718,7 @@ bool seqplay::setJointAnglesSequenceFull(std::vector i_pos, std:: } const double *q_next = i_pos[i+1]; const double *q_prev = i==0?x:i_pos[i-1]; - for (unsigned int j = 0; j < m_dof; j++) { + for (int j = 0; j < m_dof; j++) { double d0, d1, v0, v1; d0 = (q[j] - q_prev[j]); d1 = (q_next[j] - q[j]); @@ -731,7 +731,7 @@ bool seqplay::setJointAnglesSequenceFull(std::vector i_pos, std:: } } } else { - for (unsigned int j = 0; j < m_dof; j++) { v[j] = 0.0; } + for (int j = 0; j < m_dof; j++) { v[j] = 0.0; } } } @@ -773,13 +773,21 @@ bool seqplay::setJointAnglesSequenceOfGroup(const char *gname, std::vectorindices.size(); - double x[len], v[len]; + // playPatternOfGroup double q[m_dof], dq[m_dof]; - i->inter->get(q, dq, false); - i->inter->set(q, dq); + interpolators[Q]->get(q, dq, false); // fill all q,dq data + std::map::iterator it; + for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){ + groupInterpolator *gi = it->second; + if (gi) gi->get(q, dq, false); + } + // extract currnet limb data + double x[len], v[len]; i->extract(x, q); i->extract(v, dq); + // override currnet goal i->inter->clear(); + i->inter->go(x,v,interpolators[Q]->deltaT()); const double *q_curr=NULL; for (unsigned int j=0; jstate == groupInterpolator::created){ + double q[m_dof], dq[m_dof]; interpolators[Q]->get(q, dq, false); std::map::iterator it; for (it=groupInterpolators.begin(); it!=groupInterpolators.end(); it++){ groupInterpolator *gi = it->second; if (gi) gi->get(q, dq, false); } + double x[i->indices.size()], v[i->indices.size()]; i->extract(x, q); i->extract(v, dq); i->inter->go(x,v,interpolators[Q]->deltaT()); diff --git a/rtc/SequencePlayer/timeUtil.cpp b/rtc/SequencePlayer/timeUtil.cpp index b45d9fa48e0..989981a4636 100644 --- a/rtc/SequencePlayer/timeUtil.cpp +++ b/rtc/SequencePlayer/timeUtil.cpp @@ -1,6 +1,7 @@ #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" #include "timeUtil.h" +#include tick_t get_tick() @@ -9,6 +10,14 @@ tick_t get_tick() LARGE_INTEGER t; QueryPerformanceCounter(&t); return t.QuadPart; +#elif defined(__ARM_ARCH_7A__) + uint32_t r = 0; + asm volatile("mrc p15, 0, %0, c9, c13, 0" : "=r"(r) ); + return r; +#elif defined(__AARCH64EL__) + uint64_t b; + asm volatile( "mrs %0, pmccntr_el0" : "=r"(b) :: "memory" ); + return b; #else unsigned int l=0,h=0; __asm__ __volatile__("rdtsc" : "=a" (l), "=d" (h)); diff --git a/rtc/ServoController/CMakeLists.txt b/rtc/ServoController/CMakeLists.txt index 30cb60a82d0..ff79da72c69 100644 --- a/rtc/ServoController/CMakeLists.txt +++ b/rtc/ServoController/CMakeLists.txt @@ -12,6 +12,6 @@ add_executable(testServoSerial testServoSerial.cpp) set(target ServoController ServoControllerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ServoController/ServoController.cpp b/rtc/ServoController/ServoController.cpp index 74c60e2c873..0cb117e2622 100644 --- a/rtc/ServoController/ServoController.cpp +++ b/rtc/ServoController/ServoController.cpp @@ -11,7 +11,7 @@ #include #include #include "ServoController.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include "ServoSerial.h" @@ -92,12 +92,12 @@ RTC::ReturnCode_t ServoController::onInitialize() return RTC::RTC_ERROR; } servo_id.resize(servo_ids.size()); - for(int i = 0; i < servo_ids.size(); i++) { + for(unsigned int i = 0; i < servo_ids.size(); i++) { coil::stringTo(servo_id[i], servo_ids[i].c_str()); } std::cout << m_profile.instance_name << ": servo_id : "; - for(int i = 0; i < servo_id.size(); i++) { + for(unsigned int i = 0; i < servo_id.size(); i++) { std::cerr << servo_id[i] << " "; } std::cerr << std::endl; @@ -113,12 +113,12 @@ RTC::ReturnCode_t ServoController::onInitialize() std::cerr << "\e[1;31m[ERROR] " << m_profile.instance_name << ": servo.id and servo.offset property must have same length\e[0m" << std::endl; return RTC::RTC_ERROR; } - for(int i = 0; i < servo_offsets.size(); i++) { + for(unsigned int i = 0; i < servo_offsets.size(); i++) { coil::stringTo(servo_offset[i], servo_offsets[i].c_str()); } std::cout << m_profile.instance_name << ": servo_offset : "; - for(int i = 0; i < servo_offset.size(); i++) { + for(unsigned int i = 0; i < servo_offset.size(); i++) { std::cerr << servo_offset[i] << " "; } std::cerr << std::endl; @@ -134,12 +134,12 @@ RTC::ReturnCode_t ServoController::onInitialize() std::cerr << "\e[1;31m[ERROR] " << m_profile.instance_name << ": servo.id and servo.dir property must have same length\e[0m" << std::endl; return RTC::RTC_ERROR; } - for(int i = 0; i < servo_dirs.size(); i++) { + for(unsigned int i = 0; i < servo_dirs.size(); i++) { coil::stringTo(servo_dir[i], servo_dirs[i].c_str()); } std::cout << m_profile.instance_name << ": servo_dir : "; - for(int i = 0; i < servo_dir.size(); i++) { + for(unsigned int i = 0; i < servo_dir.size(); i++) { std::cerr << servo_dir[i] << " "; } std::cerr << std::endl; @@ -231,7 +231,7 @@ bool ServoController::setJointAngle(short id, double angle, double tm) { if ( ! serial ) return true; double rad = angle * M_PI / 180; - for(int i=0; isetPosition(id,rad+servo_offset[i], tm); } return true; @@ -244,7 +244,7 @@ bool ServoController::setJointAngles(const OpenHRP::ServoControllerService::dSeq int id[servo_id.size()]; double tms[servo_id.size()]; double rad[servo_id.size()]; - for( int i = 0; i < servo_id.size(); i++ ) { + for( unsigned int i = 0; i < servo_id.size(); i++ ) { id[i] = servo_id[i]; tms[i] = tm; rad[i] = (angles.get_buffer()[i]*servo_dir[i]+servo_offset[i]); @@ -262,7 +262,7 @@ bool ServoController::getJointAngle(short id, double &angle) if ( ! serial ) return true; int ret = serial->getPosition(id, &angle); - for(int i=0; ilength(servo_id.size()); - for(int i=0; i < servo_id.size(); i++){ + for(unsigned int i=0; i < servo_id.size(); i++){ ret = serial->getPosition(servo_id[i], &(angles->get_buffer()[i])); if (ret < 0) return false; } @@ -313,7 +313,7 @@ bool ServoController::setJointAnglesOfGroup(const char *gname, const OpenHRP::Se if ( ! serial ) return true; if ( joint_groups.find(gname) != joint_groups.end()) { - int len = joint_groups[gname].size(); + unsigned int len = joint_groups[gname].size(); if ( angles.length() != len ) { std::cerr << "[ERROR] " << m_profile.instance_name << ": size of servo.id(" << angles.length() << ") is not correct, expected" << len << std::endl; return false; @@ -321,11 +321,11 @@ bool ServoController::setJointAnglesOfGroup(const char *gname, const OpenHRP::Se int id[len]; double tms[len]; double rad[len]; - for( int i = 0; i < len; i++ ) { + for( unsigned int i = 0; i < len; i++ ) { id[i] = joint_groups[gname][i]; tms[i] = tm; double offset, dir; - for( int j = 0; j < servo_id.size(); j++ ) { + for( unsigned int j = 0; j < servo_id.size(); j++ ) { if ( servo_id[j] == id[i]) { offset = servo_offset[j]; dir = servo_dir[j]; diff --git a/rtc/ServoController/ServoController.h b/rtc/ServoController/ServoController.h index 451c92dc13b..df4acd6c827 100644 --- a/rtc/ServoController/ServoController.h +++ b/rtc/ServoController/ServoController.h @@ -10,6 +10,8 @@ #ifndef SERVO_CONTROLLER_H #define SERVO_CONTROLLER_H +#include +#include #include #include #include diff --git a/rtc/ServoController/ServoControllerService_impl.h b/rtc/ServoController/ServoControllerService_impl.h index 3c469c66c4e..29d5cf779a6 100644 --- a/rtc/ServoController/ServoControllerService_impl.h +++ b/rtc/ServoController/ServoControllerService_impl.h @@ -2,7 +2,7 @@ #ifndef __SERVOCONTROLLER_SERVICE_H__ #define __SERVOCONTROLLER_SERVICE_H__ -#include "ServoControllerService.hh" +#include "hrpsys/idl/ServoControllerService.hh" using namespace OpenHRP; diff --git a/rtc/ServoController/ServoSerial.h b/rtc/ServoController/ServoSerial.h index e3c8a9e9842..98cef34190a 100644 --- a/rtc/ServoController/ServoSerial.h +++ b/rtc/ServoController/ServoSerial.h @@ -24,7 +24,7 @@ class ServoSerial { public: int fd; - ServoSerial(char *devname) { + ServoSerial(const char *devname) { fd = open(devname, O_RDWR); if (fd<0) { char *pmesg = strerror(errno); @@ -329,13 +329,13 @@ class ServoSerial { if ( length * count > 0 ) { memcpy((void *)(&(packet[7])), (void *)data, length*count); } - for(unsigned int i = 2; i < 7 + length*count; i++){ + for(int i = 2; i < 7 + length*count; i++){ sum ^= packet[i]; } packet[7+length*count] = sum; fprintf (stderr, "[ServoSerial] sending : "); - for(unsigned int i = 0; i < 7 + length*count + 1; i++){ + for(int i = 0; i < 7 + length*count + 1; i++){ fprintf(stderr, "%02X ", packet[i]); } fprintf(stderr, " - "); diff --git a/rtc/Simulator/CMakeLists.txt b/rtc/Simulator/CMakeLists.txt index f555b545248..195be9cc0fe 100644 --- a/rtc/Simulator/CMakeLists.txt +++ b/rtc/Simulator/CMakeLists.txt @@ -13,6 +13,6 @@ target_link_libraries(SimulatorComp ${libraries}) set(target Simulator SimulatorComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Simulator/RTCBody.cpp b/rtc/Simulator/RTCBody.cpp index fcd5faf3cfd..06ecd41f8f1 100644 --- a/rtc/Simulator/RTCBody.cpp +++ b/rtc/Simulator/RTCBody.cpp @@ -23,7 +23,7 @@ RTCBody::~RTCBody() void RTCBody::createPorts(RTC::DataFlowComponentBase *comp) { // input ports - for (int i=0; iisHighGainMode){ @@ -88,7 +88,7 @@ void RTCBody::input() do { m_qCmdIn.read(); }while (m_qCmdIn.isNew()); - for (int i=0; iq = m_qCmd.data[l->jointId]; @@ -99,7 +99,7 @@ void RTCBody::input() do { m_dqCmdIn.read(); }while (m_dqCmdIn.isNew()); - for (int i=0; idq = m_dqCmd.data[l->jointId]; @@ -110,7 +110,7 @@ void RTCBody::input() do { m_ddqCmdIn.read(); }while (m_ddqCmdIn.isNew()); - for (int i=0; iddq = m_ddqCmd.data[l->jointId]; @@ -122,7 +122,7 @@ void RTCBody::input() do { m_tauIn.read(); }while (m_tauIn.isNew()); - for (int i=0; iu = m_tau.data[l->jointId]; @@ -135,7 +135,7 @@ void RTCBody::input() void RTCBody::output(OpenHRP::RobotState& state) { if (numJoints() > 0){ - for (int i=0; ijointId] = l->q; diff --git a/rtc/Simulator/RTCBody.h b/rtc/Simulator/RTCBody.h index d96f0d59889..6d3f0db8e5b 100644 --- a/rtc/Simulator/RTCBody.h +++ b/rtc/Simulator/RTCBody.h @@ -1,13 +1,15 @@ #ifndef __RTCBODY_H__ #define __RTCBODY_H__ +#include +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include #include #include #include -#include "HRPDataTypes.hh" class RTCBody : public hrp::Body { diff --git a/rtc/Simulator/Simulator.cpp b/rtc/Simulator/Simulator.cpp index 9b714f1f31a..b84aef11113 100644 --- a/rtc/Simulator/Simulator.cpp +++ b/rtc/Simulator/Simulator.cpp @@ -14,7 +14,7 @@ #include #include #include -#include "util/Project.h" +#include "hrpsys/util/Project.h" // Module specification // @@ -278,7 +278,7 @@ RTC::ReturnCode_t Simulator::onExecute(RTC::UniqueId ec_id) for (unsigned int i=0; iinput(); if (m_kinematicsOnly){ - for(int i=0; i < m_world.numBodies(); ++i){ + for(unsigned int i=0; i < m_world.numBodies(); ++i){ m_world.body(i)->calcForwardKinematics(); } m_world.setCurrentTime(m_world.currentTime() + m_world.timeStep()); diff --git a/rtc/Simulator/Simulator.h b/rtc/Simulator/Simulator.h index 4c9e1ed9eba..d9ae49f6322 100644 --- a/rtc/Simulator/Simulator.h +++ b/rtc/Simulator/Simulator.h @@ -10,6 +10,8 @@ #ifndef SIMULATOR_H #define SIMULATOR_H +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include @@ -21,7 +23,6 @@ #include #include -#include "HRPDataTypes.hh" #include "RTCBody.h" // Service implementation headers diff --git a/rtc/SoftErrorLimiter/CMakeLists.txt b/rtc/SoftErrorLimiter/CMakeLists.txt index 1cdd060a756..8fd90c5edb2 100644 --- a/rtc/SoftErrorLimiter/CMakeLists.txt +++ b/rtc/SoftErrorLimiter/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(SoftErrorLimiterComp ${libs}) set(target SoftErrorLimiter SoftErrorLimiterComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp index 31fa15ba4ff..1073313c35c 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp @@ -8,18 +8,17 @@ */ #include "SoftErrorLimiter.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" +#include "hrpsys/idl/RobotHardwareService.hh" #include #include -#include "RobotHardwareService.hh" #include #include #include +#include #define deg2rad(x)((x)*M_PI/180) -#include "beep.h" - // Module specification // static const char* softerrorlimiter_spec[] = @@ -40,6 +39,17 @@ static const char* softerrorlimiter_spec[] = }; // +static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm) +{ + int pre = os.precision(); + os.setf(std::ios::fixed); + os << std::setprecision(6) + << (tm.sec + tm.nsec/1e9) + << std::setprecision(pre); + os.unsetf(std::ios::fixed); + return os; +} + SoftErrorLimiter::SoftErrorLimiter(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager), // @@ -48,10 +58,12 @@ SoftErrorLimiter::SoftErrorLimiter(RTC::Manager* manager) m_servoStateIn("servoStateIn", m_servoState), m_qOut("q", m_qRef), m_servoStateOut("servoStateOut", m_servoState), + m_beepCommandOut("beepCommand", m_beepCommand), m_SoftErrorLimiterServicePort("SoftErrorLimiterService"), // m_debugLevel(0), - dummy(0) + dummy(0), + is_beep_port_connected(false) { init_beep(); start_beep(3136); @@ -83,6 +95,7 @@ RTC::ReturnCode_t SoftErrorLimiter::onInitialize() // Set OutPort buffer addOutPort("q", m_qOut); addOutPort("servoState", m_servoStateOut); + addOutPort("beepCommand", m_beepCommandOut); // Set service provider to Ports m_SoftErrorLimiterServicePort.registerProvider("service0", "SoftErrorLimiterService", m_service0); @@ -117,7 +130,7 @@ RTC::ReturnCode_t SoftErrorLimiter::onInitialize() if (!m_robot->init()) return RTC::RTC_ERROR; m_service0.setRobot(m_robot); m_servoState.data.length(m_robot->numJoints()); - for(int i = 0; i < m_robot->numJoints(); i++) { + for(unsigned int i = 0; i < m_robot->numJoints(); i++) { m_servoState.data[i].length(1); int status = 0; status |= 1<< OpenHRP::RobotHardwareService::CALIB_STATE_SHIFT; @@ -135,10 +148,24 @@ RTC::ReturnCode_t SoftErrorLimiter::onInitialize() debug_print_freq = static_cast(0.2/dt); // once per 0.2 [s] /* If you print debug message for all controller loop, please comment in here */ // debug_print_freq = 1; + m_beepCommand.data.length(bc.get_num_beep_info()); // load joint limit table hrp::readJointLimitTableFromProperties (joint_limit_tables, m_robot, prop["joint_limit_table"], std::string(m_profile.instance_name)); + // read ignored joint + m_joint_mask.resize(m_robot->numJoints(), false); + coil::vstring ijoints = coil::split(prop["mask_joint_limit"], ","); + for(int i = 0; i < ijoints.size(); i++) { + hrp::Link *lk = m_robot->link(std::string(ijoints[i])); + if((!!lk) && (lk->jointId >= 0)) { + std::cout << "[" << m_profile.instance_name << "] " + << "disable ErrorLimit, joint : " << ijoints[i] + << " (id = " << lk->jointId << ")" << std::endl; + m_joint_mask[lk->jointId] = true; + } + } + return RTC::RTC_OK; } @@ -184,6 +211,17 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) static bool debug_print_error_first = false; loop ++; + // Connection check of m_beepCommand to BeeperRTC + // If m_beepCommand is not connected to BeeperRTC and sometimes, check connection. + // If once connection is found, never check connection. + if (!is_beep_port_connected && (loop % 500 == 0) ) { + if ( m_beepCommandOut.connectors().size() > 0 ) { + is_beep_port_connected = true; + quit_beep(); + std::cerr << "[" << m_profile.instance_name<< "] beepCommand data port connection found! Use BeeperRTC." << std::endl; + } + } + if (m_qRefIn.isNew()) { m_qRefIn.read(); } @@ -217,13 +255,13 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) static std::vector prev_angle; if ( prev_angle.size() != m_qRef.data.length() ) { // initialize prev_angle prev_angle.resize(m_qRef.data.length(), 0); - for ( int i = 0; i < m_qRef.data.length(); i++ ){ + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){ prev_angle[i] = m_qCurrent.data[i]; } } std::vector servo_state; servo_state.resize(m_qRef.data.length(), 0); - for ( int i = 0; i < m_qRef.data.length(); i++ ){ + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){ servo_state[i] = (m_servoState.data[i][0] & OpenHRP::RobotHardwareService::SERVO_STATE_MASK) >> OpenHRP::RobotHardwareService::SERVO_STATE_SHIFT; // enum SwitchStatus {SWITCH_ON, SWITCH_OFF}; } @@ -244,7 +282,8 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) */ // Velocity limitation for reference joint angles - for ( int i = 0; i < m_qRef.data.length(); i++ ){ + for ( unsigned int i = 0; i < m_qRef.data.length(); i++ ){ + if(m_joint_mask[i]) continue; // Determin total upper-lower limit considering velocity, position, and error limits. // e.g., // total lower limit = max (vel, pos, err) <= severest lower limit @@ -257,17 +296,22 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) // fixed joint has ulimit = vlimit if ( servo_state[i] == 1 && (lvlimit < uvlimit) && ((lvlimit > qvel) || (uvlimit < qvel)) ) { if (loop % debug_print_freq == 0 || debug_print_velocity_first ) { - std::cerr << "[" << m_profile.instance_name<< "] velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel + std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm + << "] velocity limit over " << m_robot->joint(i)->name << "(" << i << "), qvel=" << qvel << ", lvlimit =" << lvlimit << ", uvlimit =" << uvlimit - << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF") << std::endl; + << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF"); } + double limited; // fix joint angle if ( lvlimit > qvel ) { - total_lower_limit = std::max(prev_angle[i] + lvlimit * dt, total_lower_limit); + limited = total_lower_limit = std::max(prev_angle[i] + lvlimit * dt, total_lower_limit); } if ( uvlimit < qvel ) { - total_upper_limit = std::min(prev_angle[i] + uvlimit * dt, total_upper_limit); + limited = total_upper_limit = std::min(prev_angle[i] + uvlimit * dt, total_upper_limit); + } + if (loop % debug_print_freq == 0 || debug_print_velocity_first ) { + std::cerr << ", q(limited) = " << limited << std::endl; } velocity_limit_error = true; } @@ -286,18 +330,23 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) bool servo_limit_state = (llimit < ulimit) && ((llimit > m_qRef.data[i]) || (ulimit < m_qRef.data[i])); if ( servo_state[i] == 1 && servo_limit_state ) { if (loop % debug_print_freq == 0 || debug_print_position_first) { - std::cerr << "[" << m_profile.instance_name<< "] position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i] + std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm + << "] position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i] << ", llimit =" << llimit << ", ulimit =" << ulimit << ", servo_state = " << ( servo_state[i] ? "ON" : "OFF") - << ", prev_angle = " << prev_angle[i] << std::endl; + << ", prev_angle = " << prev_angle[i]; } + double limited; // fix joint angle if ( llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) { // ref < llimit and prev < ref -> OK - total_lower_limit = std::max(llimit, total_lower_limit); + limited = total_lower_limit = std::max(llimit, total_lower_limit); } if ( ulimit < m_qRef.data[i] && prev_angle[i] < m_qRef.data[i] ) { // ulimit < ref and ref < prev -> OK - total_upper_limit = std::min(ulimit, total_upper_limit); + limited = total_upper_limit = std::min(ulimit, total_upper_limit); + } + if (loop % debug_print_freq == 0 || debug_print_position_first ) { + std::cerr << ", q(limited) = " << limited << std::endl; } m_servoState.data[i][0] |= (0x200 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT); position_limit_error = true; @@ -305,23 +354,28 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) } // Servo error limitation between reference joint angles and actual joint angles + // pos_vel_limited_angle is current output joint angle which arleady finished position limit and velocity limit. + // Check and limit error between pos_vel_limited_angle and current actual joint angle. { + double pos_vel_limited_angle = std::min(total_upper_limit, std::max(total_lower_limit, m_qRef.data[i])); double limit = m_robot->m_servoErrorLimit[i]; - double error = m_qRef.data[i] - m_qCurrent.data[i]; + double error = pos_vel_limited_angle - m_qCurrent.data[i]; if ( servo_state[i] == 1 && fabs(error) > limit ) { if (loop % debug_print_freq == 0 || debug_print_error_first ) { - std::cerr << "[" << m_profile.instance_name<< "] error limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i] + std::cerr << "[" << m_profile.instance_name<< "] [" << m_qRef.tm + << "] error limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << pos_vel_limited_angle << ", qCurrent=" << m_qCurrent.data[i] << " " << ", Error=" << error << " > " << limit << " (limit)" << ", servo_state = " << ( 1 ? "ON" : "OFF"); } + double limited; if ( error > limit ) { - total_upper_limit = std::min(m_qCurrent.data[i] + limit, total_upper_limit); + limited = total_upper_limit = std::min(m_qCurrent.data[i] + limit, total_upper_limit); } else { - total_lower_limit = std::max(m_qCurrent.data[i] - limit, total_lower_limit); + limited = total_lower_limit = std::max(m_qCurrent.data[i] - limit, total_lower_limit); } if (loop % debug_print_freq == 0 || debug_print_error_first ) { - std::cerr << ", q=" << m_qRef.data[i] << std::endl; + std::cerr << ", q(limited) = " << limited << std::endl; } m_servoState.data[i][0] |= (0x040 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT); soft_limit_error = true; @@ -338,16 +392,34 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) // Beep sound if ( soft_limit_error ) { // play beep - if ( loop % soft_limit_error_beep_freq == 0 ) start_beep(3136, soft_limit_error_beep_freq*0.8); + if (is_beep_port_connected) { + if ( loop % soft_limit_error_beep_freq == 0 ) bc.startBeep(3136, soft_limit_error_beep_freq*0.8); + else bc.stopBeep(); + } else { + if ( loop % soft_limit_error_beep_freq == 0 ) start_beep(3136, soft_limit_error_beep_freq*0.8); + } }else if ( position_limit_error || velocity_limit_error ) { // play beep - if ( loop % position_limit_error_beep_freq == 0 ) start_beep(3520, position_limit_error_beep_freq*0.8); + if (is_beep_port_connected) { + if ( loop % position_limit_error_beep_freq == 0 ) bc.startBeep(3520, position_limit_error_beep_freq*0.8); + else bc.stopBeep(); + } else { + if ( loop % position_limit_error_beep_freq == 0 ) start_beep(3520, position_limit_error_beep_freq*0.8); + } } else { - stop_beep(); + if (is_beep_port_connected) { + bc.stopBeep(); + } else { + stop_beep(); + } } m_qOut.write(); m_servoStateOut.write(); } else { - start_beep(3136); + if (is_beep_port_connected) { + bc.startBeep(3136); + } else { + start_beep(3136); + } if ( loop % 100 == 1 ) { std::cerr << "SoftErrorLimiter is not working..." << std::endl; std::cerr << " m_qRef " << m_qRef.data.length() << std::endl; @@ -355,6 +427,10 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) std::cerr << " m_servoState " << m_servoState.data.length() << std::endl; } } + if (is_beep_port_connected) { + bc.setDataPort(m_beepCommand); + if (bc.isWritable()) m_beepCommandOut.write(); + } return RTC::RTC_OK; } diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.h b/rtc/SoftErrorLimiter/SoftErrorLimiter.h index 9895fb15561..529737f9d9a 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.h +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.h @@ -10,18 +10,20 @@ #ifndef SOFT_ERROR_LIMITER_H #define SOFT_ERROR_LIMITER_H +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include #include #include #include -#include "HRPDataTypes.hh" #include "JointLimitTable.h" // Service implementation headers // #include "SoftErrorLimiterService_impl.h" +#include "beep.h" // @@ -107,6 +109,7 @@ class SoftErrorLimiter TimedDoubleSeq m_qRef; TimedDoubleSeq m_qCurrent; OpenHRP::TimedLongSeqSeq m_servoState; + TimedLongSeq m_beepCommand; // DataInPort declaration // @@ -120,6 +123,7 @@ class SoftErrorLimiter // OutPort m_qOut; OutPort m_servoStateOut; + OutPort m_beepCommandOut; // @@ -143,9 +147,14 @@ class SoftErrorLimiter private: boost::shared_ptr m_robot; std::map joint_limit_tables; + std::vector m_joint_mask; unsigned int m_debugLevel; int dummy, position_limit_error_beep_freq, soft_limit_error_beep_freq, debug_print_freq; double dt; + BeepClient bc; + // Since this RTC is stable RTC, we support both direct beeping from this RTC and beepring through BeeperRTC. + // If m_beepCommand is connected to BeeperRTC, is_beep_port_connected is true. + bool is_beep_port_connected; }; diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.txt b/rtc/SoftErrorLimiter/SoftErrorLimiter.txt index 3d919106bde..8b7ade50b17 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.txt +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.txt @@ -31,6 +31,8 @@ At the same time, this component makes beep sound 2 times per 1[s]. If the cause of errors are removed, error flags and beep sound are removed. +Since this RTC is stable RTC, we support both direct beeping from this RTC and beepring through BeeperRTC. + diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiterService_impl.h b/rtc/SoftErrorLimiter/SoftErrorLimiterService_impl.h index 9e0ab3d2f99..9095086e264 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiterService_impl.h +++ b/rtc/SoftErrorLimiter/SoftErrorLimiterService_impl.h @@ -2,7 +2,7 @@ #ifndef __SOFT_ERROR_LIMITER_SERVICE_H__ #define __SOFT_ERROR_LIMITER_SERVICE_H__ -#include "SoftErrorLimiterService.hh" +#include "hrpsys/idl/SoftErrorLimiterService.hh" #include "robot.h" class SoftErrorLimiterService_impl diff --git a/rtc/SoftErrorLimiter/beep.h b/rtc/SoftErrorLimiter/beep.h index 46ea5554b99..971acf23349 100644 --- a/rtc/SoftErrorLimiter/beep.h +++ b/rtc/SoftErrorLimiter/beep.h @@ -1,4 +1,50 @@ +#ifndef BEEP_H +#define BEEP_H + void init_beep(); void start_beep(int freq, int length=50); void stop_beep(); void quit_beep(); + +typedef enum {BEEP_INFO_START, BEEP_INFO_FREQ, BEEP_INFO_LENGTH, NUM_BEEP_INFO} beep_info; + +// Beep client to send command to BeeperRTC +#include +class BeepClient +{ + private: + bool is_start_beep, prev_is_start_beep; + int freq, length; + public: + BeepClient () : is_start_beep(false), prev_is_start_beep(false) {}; + ~BeepClient () {}; + int get_num_beep_info () const { return NUM_BEEP_INFO; }; + void startBeep (const int _freq, const int _length = 50) + { + prev_is_start_beep = is_start_beep; + is_start_beep = true; + freq = _freq; + length = _length; + }; + void stopBeep () + { + prev_is_start_beep = is_start_beep; + is_start_beep = false; + freq = 1; // dummy + length = 0; // dummy + }; + bool isWritable () const + { + // Write data port to overwrite and pass through between client RTCs. + // If currently "start" or changed to "stop", write data port. + // If keep "stop", do not write data port. + return (is_start_beep || prev_is_start_beep); + }; + void setDataPort (RTC::TimedLongSeq& out_data) + { + out_data.data[BEEP_INFO_START] = (is_start_beep?1:0); + out_data.data[BEEP_INFO_FREQ] = freq; + out_data.data[BEEP_INFO_LENGTH] = length; + }; +}; +#endif // BEEP_H diff --git a/rtc/SoftErrorLimiter/robot.cpp b/rtc/SoftErrorLimiter/robot.cpp index bf4f5fb879d..51a3bcd0ed1 100644 --- a/rtc/SoftErrorLimiter/robot.cpp +++ b/rtc/SoftErrorLimiter/robot.cpp @@ -1,5 +1,6 @@ #include "robot.h" -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" +#include #define DEFAULT_ANGLE_ERROR_LIMIT (0.2 - 0.02) // [rad] @@ -11,7 +12,7 @@ robot::~robot() { } bool robot::init() { m_servoErrorLimit.resize(numJoints()); - for (int i=0; ijointId >= 0)){ m_servoErrorLimit[l->jointId] = i_limit; + std::cerr << "[el] setServoErrorLimit " << i_limit << "[rad] for " << i_jname << std::endl; }else{ + std::cerr << "[el] Invalid joint name of jointId of setServoErrorLimit name:" << i_jname << " jointId:" << l->jointId << "!" << std::endl; return false; } return true; diff --git a/rtc/Stabilizer/CMakeLists.txt b/rtc/Stabilizer/CMakeLists.txt index 57426c1afd6..39833282931 100644 --- a/rtc/Stabilizer/CMakeLists.txt +++ b/rtc/Stabilizer/CMakeLists.txt @@ -1,4 +1,4 @@ -option(BUILD_STABILIZER "Build Stabilizer RTC" ON) +option(BUILD_STABILIZER "Build Stabilizer RTC" OFF) if(NOT BUILD_STABILIZER) return() @@ -25,20 +25,26 @@ target_link_libraries(StabilizerComp ${libs}) set(target Stabilizer StabilizerComp) -add_executable(testTwoDofController testTwoDofController.cpp ${comp_sources}) +add_executable(testTwoDofController testTwoDofController.cpp TwoDofController.cpp Integrator.cpp) target_link_libraries(testTwoDofController ${libs}) add_executable(testZMPDistributor testZMPDistributor.cpp ZMPDistributor.h ../ImpedanceController/JointPathEx.cpp) target_link_libraries(testZMPDistributor ${libs}) set(target Stabilizer StabilizerComp testTwoDofController testZMPDistributor) -add_test(testZMPDistributorHRP2JSKTest0 testZMPDistributor --hrp2jsk --test0 --use-gnuplot false) -add_test(testZMPDistributorHRP2JSKTest1 testZMPDistributor --hrp2jsk --test1 --use-gnuplot false) -add_test(testZMPDistributorHRP2JSKTest2 testZMPDistributor --hrp2jsk --test2 --use-gnuplot false) -add_test(testZMPDistributorJAXONREDTest0 testZMPDistributor --jaxon_red --test0 --use-gnuplot false) -add_test(testZMPDistributorJAXONREDTest1 testZMPDistributor --jaxon_red --test1 --use-gnuplot false) -add_test(testZMPDistributorJAXONREDTest2 testZMPDistributor --jaxon_red --test2 --use-gnuplot false) +add_test(testZMPDistributor_HRP2JSK_EEFM_Test0 testZMPDistributor --hrp2jsk --test0 --use-gnuplot false) +add_test(testZMPDistributor_HRP2JSK_EEFM_Test1 testZMPDistributor --hrp2jsk --test1 --use-gnuplot false) +add_test(testZMPDistributor_HRP2JSK_EEFM_Test2 testZMPDistributor --hrp2jsk --test2 --use-gnuplot false) +add_test(testZMPDistributor_JAXONRED_EEFM_Test0 testZMPDistributor --jaxon_red --test0 --use-gnuplot false) +add_test(testZMPDistributor_JAXONRED_EEFM_Test1 testZMPDistributor --jaxon_red --test1 --use-gnuplot false) +add_test(testZMPDistributor_JAXONRED_EEFM_Test2 testZMPDistributor --jaxon_red --test2 --use-gnuplot false) +add_test(testZMPDistributor_HRP2JSK_EEFMQPCOP_Test0 testZMPDistributor --hrp2jsk --test0 --use-gnuplot false --distribution-algorithm EEFMQPCOP) +add_test(testZMPDistributor_HRP2JSK_EEFMQPCOP_Test1 testZMPDistributor --hrp2jsk --test1 --use-gnuplot false --distribution-algorithm EEFMQPCOP) +add_test(testZMPDistributor_HRP2JSK_EEFMQPCOP_Test2 testZMPDistributor --hrp2jsk --test2 --use-gnuplot false --distribution-algorithm EEFMQPCOP) +add_test(testZMPDistributor_JAXONRED_EEFMQPCOP_Test0 testZMPDistributor --jaxon_red --test0 --use-gnuplot false --distribution-algorithm EEFMQPCOP) +add_test(testZMPDistributor_JAXONRED_EEFMQPCOP_Test1 testZMPDistributor --jaxon_red --test1 --use-gnuplot false --distribution-algorithm EEFMQPCOP) +add_test(testZMPDistributor_JAXONRED_EEFMQPCOP_Test2 testZMPDistributor --jaxon_red --test2 --use-gnuplot false --distribution-algorithm EEFMQPCOP) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 34f10115b58..f6fd157986b 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -12,11 +12,19 @@ #include #include #include "Stabilizer.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include #include typedef coil::Guard Guard; + +#ifndef deg2rad +#define deg2rad(x) ((x) * M_PI / 180.0) +#endif +#ifndef rad2deg +#define rad2deg(rad) (rad * 180 / M_PI) +#endif + // Module specification // static const char* stabilizer_spec[] = @@ -37,6 +45,17 @@ static const char* stabilizer_spec[] = }; // +static std::ostream& operator<<(std::ostream& os, const struct RTC::Time &tm) +{ + int pre = os.precision(); + os.setf(std::ios::fixed); + os << std::setprecision(6) + << (tm.sec + tm.nsec/1e9) + << std::setprecision(pre); + os.unsetf(std::ios::fixed); + return os; +} + static double switching_inpact_absorber(double force, double lower_th, double upper_th); Stabilizer::Stabilizer(RTC::Manager* manager) @@ -50,6 +69,7 @@ Stabilizer::Stabilizer(RTC::Manager* manager) m_basePosIn("basePosIn", m_basePos), m_baseRpyIn("baseRpyIn", m_baseRpy), m_contactStatesIn("contactStates", m_contactStates), + m_toeheelRatioIn("toeheelRatio", m_toeheelRatio), m_controlSwingSupportTimeIn("controlSwingSupportTime", m_controlSwingSupportTime), m_qRefSeqIn("qRefSeq", m_qRefSeq), m_walkingStatesIn("walkingStates", m_walkingStates), @@ -59,6 +79,8 @@ Stabilizer::Stabilizer(RTC::Manager* manager) m_zmpOut("zmp", m_zmp), m_refCPOut("refCapturePoint", m_refCP), m_actCPOut("actCapturePoint", m_actCP), + m_diffCPOut("diffCapturePoint", m_diffCP), + m_diffFootOriginExtMomentOut("diffFootOriginExtMoment", m_diffFootOriginExtMoment), m_actContactStatesOut("actContactStates", m_actContactStates), m_COPInfoOut("COPInfo", m_COPInfo), m_emergencySignalOut("emergencySignal", m_emergencySignal), @@ -109,6 +131,7 @@ RTC::ReturnCode_t Stabilizer::onInitialize() addInPort("basePosIn", m_basePosIn); addInPort("baseRpyIn", m_baseRpyIn); addInPort("contactStates", m_contactStatesIn); + addInPort("toeheelRatio", m_toeheelRatioIn); addInPort("controlSwingSupportTime", m_controlSwingSupportTimeIn); addInPort("qRefSeq", m_qRefSeqIn); addInPort("walkingStates", m_walkingStatesIn); @@ -120,6 +143,8 @@ RTC::ReturnCode_t Stabilizer::onInitialize() addOutPort("zmp", m_zmpOut); addOutPort("refCapturePoint", m_refCPOut); addOutPort("actCapturePoint", m_actCPOut); + addOutPort("diffCapturePoint", m_diffCPOut); + addOutPort("diffStaticBalancePointOffset", m_diffFootOriginExtMomentOut); addOutPort("actContactStates", m_actContactStatesOut); addOutPort("COPInfo", m_COPInfoOut); addOutPort("emergencySignal", m_emergencySignalOut); @@ -169,33 +194,53 @@ RTC::ReturnCode_t Stabilizer::onInitialize() return RTC::RTC_ERROR; } + // Setting for wrench data ports (real + virtual) + std::vector force_sensor_names; + + // Find names for real force sensors int npforce = m_robot->numSensors(hrp::Sensor::FORCE); - m_wrenches.resize(npforce); - m_wrenchesIn.resize(npforce); - m_ref_wrenches.resize(npforce); - m_ref_wrenchesIn.resize(npforce); - m_limbCOPOffset.resize(npforce); - m_limbCOPOffsetIn.resize(npforce); + for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); + } + + // load virtual force sensors + readVirtualForceSensorParamFromProperties(m_vfs, m_robot, prop["virtual_force_sensor"], std::string(m_profile.instance_name)); + int nvforce = m_vfs.size(); + for (unsigned int i=0; i::iterator it = m_vfs.begin(); it != m_vfs.end(); it++ ) { + if (it->second.id == i) { + force_sensor_names.push_back(it->first); + } + } + } + + int nforce = npforce + nvforce; + m_wrenches.resize(nforce); + m_wrenchesIn.resize(nforce); + m_ref_wrenches.resize(nforce); + m_ref_wrenchesIn.resize(nforce); + m_limbCOPOffset.resize(nforce); + m_limbCOPOffsetIn.resize(nforce); std::cerr << "[" << m_profile.instance_name << "] force sensor ports (" << npforce << ")" << std::endl; - for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i); - // act - m_wrenchesIn[i] = new RTC::InPort(s->name.c_str(), m_wrenches[i]); - m_wrenches[i].data.length(6); - registerInPort(s->name.c_str(), *m_wrenchesIn[i]); - // ref - m_ref_wrenchesIn[i] = new RTC::InPort(std::string(s->name+"Ref").c_str(), m_ref_wrenches[i]); - m_ref_wrenches[i].data.length(6); - registerInPort(std::string(s->name+"Ref").c_str(), *m_ref_wrenchesIn[i]); - std::cerr << "[" << m_profile.instance_name << "] name = " << s->name << std::endl; + for (unsigned int i=0; i(force_sensor_name.c_str(), m_wrenches[i]); + m_wrenches[i].data.length(6); + registerInPort(force_sensor_name.c_str(), *m_wrenchesIn[i]); + // referecen inport + m_ref_wrenchesIn[i] = new RTC::InPort(std::string(force_sensor_name+"Ref").c_str(), m_ref_wrenches[i]); + m_ref_wrenches[i].data.length(6); + registerInPort(std::string(force_sensor_name+"Ref").c_str(), *m_ref_wrenchesIn[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << force_sensor_name << std::endl; } std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << npforce << ")" << std::endl; - for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i); - std::string nm("limbCOPOffset_"+s->name); - m_limbCOPOffsetIn[i] = new RTC::InPort(nm.c_str(), m_limbCOPOffset[i]); - registerInPort(nm.c_str(), *m_limbCOPOffsetIn[i]); - std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl; + for (unsigned int i=0; i(nm.c_str(), m_limbCOPOffset[i]); + registerInPort(nm.c_str(), *m_limbCOPOffsetIn[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl; } // setting from conf file @@ -236,6 +281,17 @@ RTC::ReturnCode_t Stabilizer::onInitialize() } ikp.avoid_gain = 0.001; ikp.reference_gain = 0.01; + ikp.ik_loop_count = 3; + // For swing ee modification + ikp.target_ee_diff_p = hrp::Vector3::Zero(); + ikp.target_ee_diff_r = hrp::Matrix33::Identity(); + ikp.d_rpy_swing = hrp::Vector3::Zero(); + ikp.d_pos_swing = hrp::Vector3::Zero(); + ikp.target_ee_diff_p_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(50.0, dt, hrp::Vector3::Zero())); // [Hz] + ikp.target_ee_diff_r_filter = boost::shared_ptr >(new FirstOrderLowPassFilter(50.0, dt, hrp::Vector3::Zero())); // [Hz] + ikp.prev_d_pos_swing = hrp::Vector3::Zero(); + ikp.prev_d_rpy_swing = hrp::Vector3::Zero(); + // stikp.push_back(ikp); jpe_v.push_back(hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(ee_base), m_robot->link(ee_target), dt, false, std::string(m_profile.instance_name)))); // Fix for toe joint @@ -249,13 +305,12 @@ RTC::ReturnCode_t Stabilizer::onInitialize() } target_ee_p.push_back(hrp::Vector3::Zero()); target_ee_R.push_back(hrp::Matrix33::Identity()); + act_ee_p.push_back(hrp::Vector3::Zero()); act_ee_R.push_back(hrp::Matrix33::Identity()); - target_ee_diff_p.push_back(hrp::Vector3::Zero()); - target_ee_diff_r.push_back(hrp::Vector3::Zero()); - prev_target_ee_diff_r.push_back(hrp::Vector3::Zero()); - d_rpy_swing.push_back(hrp::Vector3::Zero()); - d_pos_swing.push_back(hrp::Vector3::Zero()); - target_ee_diff_p_filter.push_back(boost::shared_ptr >(new FirstOrderLowPassFilter(50.0, dt, hrp::Vector3::Zero()))); // [Hz] + projected_normal.push_back(hrp::Vector3::Zero()); + act_force.push_back(hrp::Vector3::Zero()); + ref_force.push_back(hrp::Vector3::Zero()); + ref_moment.push_back(hrp::Vector3::Zero()); contact_states_index_map.insert(std::pair(ee_name, i)); is_ik_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands ik => disabled, feet ik => enabled, by default is_feedback_control_enable.push_back( (ee_name.find("leg") != std::string::npos ? true : false) ); // Hands feedback control => disabled, feet feedback control => enabled, by default @@ -266,6 +321,8 @@ RTC::ReturnCode_t Stabilizer::onInitialize() prev_act_force_z.push_back(0.0); } m_contactStates.data.length(num); + m_toeheelRatio.data.length(num); + m_will_fall_counter.resize(num); } std::vector > interlocking_joints; @@ -295,9 +352,9 @@ RTC::ReturnCode_t Stabilizer::onInitialize() eefm_body_attitude_control_gain[i] = 0.5; eefm_body_attitude_control_time_const[i] = 1e5; } -#define deg2rad(x) ((x) * M_PI / 180.0) for (size_t i = 0; i < stikp.size(); i++) { STIKParam& ikp = stikp[i]; + hrp::Link* root = m_robot->link(ikp.target_name); ikp.eefm_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5); ikp.eefm_rot_time_const = hrp::Vector3(1.5, 1.5, 1.5); ikp.eefm_rot_compensation_limit = deg2rad(10.0); @@ -309,7 +366,25 @@ RTC::ReturnCode_t Stabilizer::onInitialize() ikp.eefm_swing_pos_spring_gain = hrp::Vector3(0.0, 0.0, 0.0); ikp.eefm_swing_pos_time_const = hrp::Vector3(1.5, 1.5, 1.5); ikp.eefm_ee_moment_limit = hrp::Vector3(1e4, 1e4, 1e4); // Default limit [Nm] is too large. Same as no limit. + if (ikp.ee_name.find("leg") == std::string::npos) { // Arm default + ikp.eefm_ee_forcemoment_distribution_weight = Eigen::Matrix::Zero(); + } else { // Leg default + for (size_t j = 0; j < 3; j++) { + ikp.eefm_ee_forcemoment_distribution_weight[j] = 1; // Force + ikp.eefm_ee_forcemoment_distribution_weight[j+3] = 1e-2; // Moment + } + } + ikp.max_limb_length = 0.0; + while (!root->isRoot()) { + ikp.max_limb_length += root->b.norm(); + ikp.parent_name = root->name; + root = root->parent; + } + ikp.limb_length_margin = 0.13; + ikp.support_time = 0.0; } + eefm_swing_rot_damping_gain = hrp::Vector3(20*5, 20*5, 1e5); + eefm_swing_pos_damping_gain = hrp::Vector3(33600, 33600, 7000); eefm_pos_time_const_swing = 0.08; eefm_pos_transition_time = 0.01; eefm_pos_margin_time = 0.02; @@ -319,16 +394,26 @@ RTC::ReturnCode_t Stabilizer::onInitialize() //eefm_leg_rear_margin = 0.05; //fm_wrench_alpha_blending = 1.0; // fz_alpha eefm_gravitational_acceleration = 9.80665; // [m/s^2] - eefm_ee_pos_error_p_gain = 0; - eefm_ee_rot_error_p_gain = 0; cop_check_margin = 20.0*1e-3; // [m] cp_check_margin.resize(4, 30*1e-3); // [m] + cp_offset = hrp::Vector3(0.0, 0.0, 0.0); // [m] + tilt_margin.resize(2, 30 * M_PI / 180); // [rad] contact_decision_threshold = 50; // [N] eefm_use_force_difference_control = true; + eefm_use_swing_damping = false; + eefm_swing_damping_force_thre.resize(3, 300); + eefm_swing_damping_moment_thre.resize(3, 15); initial_cp_too_large_error = true; is_walking = false; is_estop_while_walking = false; sbp_cog_offset = hrp::Vector3(0.0, 0.0, 0.0); + use_limb_stretch_avoidance = false; + use_zmp_truncation = false; + limb_stretch_avoidance_time_const = 1.5; + limb_stretch_avoidance_vlimit[0] = -100 * 1e-3 * dt; // lower limit + limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * dt; // upper limit + root_rot_compensation_limit[0] = root_rot_compensation_limit[1] = deg2rad(90.0); + detection_count_to_air = static_cast(0.0 / dt); // parameters for RUNST double ke = 0, tc = 0; @@ -362,13 +447,17 @@ RTC::ReturnCode_t Stabilizer::onInitialize() qrefv.resize(m_robot->numJoints()); transition_count = 0; loop = 0; + m_is_falling_counter = 0; + is_air_counter = 0; total_mass = m_robot->totalMass(); ref_zmp_aux = hrp::Vector3::Zero(); m_actContactStates.data.length(m_contactStates.data.length()); for (size_t i = 0; i < m_contactStates.data.length(); i++) { - contact_states.push_back(true); - prev_contact_states.push_back(true); + ref_contact_states.push_back(true); + prev_ref_contact_states.push_back(true); m_actContactStates.data[i] = false; + act_contact_states.push_back(false); + toeheel_ratio.push_back(1.0); } m_COPInfo.data.length(m_contactStates.data.length()*3); // nx, ny, fz for each end-effectors for (size_t i = 0; i < m_COPInfo.data.length(); i++) { @@ -405,6 +494,10 @@ RTC::ReturnCode_t Stabilizer::onInitialize() rel_ee_rot.reserve(stikp.size()); rel_ee_name.reserve(stikp.size()); + hrp::Sensor* sen = m_robot->sensor("gyrometer"); + if (sen == NULL) { + std::cerr << "[" << m_profile.instance_name << "] WARNING! This robot model has no GyroSensor named 'gyrometer'! " << std::endl; + } return RTC::RTC_OK; } @@ -478,7 +571,13 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) if (m_contactStatesIn.isNew()){ m_contactStatesIn.read(); for (size_t i = 0; i < m_contactStates.data.length(); i++) { - contact_states[i] = m_contactStates.data[i]; + ref_contact_states[i] = m_contactStates.data[i]; + } + } + if (m_toeheelRatioIn.isNew()){ + m_toeheelRatioIn.read(); + for (size_t i = 0; i < m_toeheelRatio.data.length(); i++) { + toeheel_ratio[i] = m_toeheelRatio.data[i]; } } if (m_controlSwingSupportTimeIn.isNew()){ @@ -531,12 +630,15 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) if ( transition_count == 0 && on_ground ) sync_2_st(); break; case MODE_ST: - if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { + if (st_algorithm != OpenHRP::StabilizerService::TPCC) { calcEEForceMomentControl(); } else { calcTPCC(); } - if ( transition_count == 0 && !on_ground ) control_mode = MODE_SYNC_TO_AIR; + if ( transition_count == 0 && !on_ground ) { + if (is_air_counter < detection_count_to_air) ++is_air_counter; + else control_mode = MODE_SYNC_TO_AIR; + } else is_air_counter = 0; break; case MODE_SYNC_TO_IDLE: sync_2_idle(); @@ -569,6 +671,19 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) m_actCP.data.z = rel_act_cp(2); m_actCP.tm = m_qRef.tm; m_actCPOut.write(); + { + hrp::Vector3 tmp_diff_cp = ref_foot_origin_rot * (ref_cp - act_cp - cp_offset); + m_diffCP.data.x = tmp_diff_cp(0); + m_diffCP.data.y = tmp_diff_cp(1); + m_diffCP.data.z = tmp_diff_cp(2); + m_diffCP.tm = m_qRef.tm; + m_diffCPOut.write(); + } + m_diffFootOriginExtMoment.data.x = diff_foot_origin_ext_moment(0); + m_diffFootOriginExtMoment.data.y = diff_foot_origin_ext_moment(1); + m_diffFootOriginExtMoment.data.z = diff_foot_origin_ext_moment(2); + m_diffFootOriginExtMoment.tm = m_qRef.tm; + m_diffFootOriginExtMomentOut.write(); m_actContactStates.tm = m_qRef.tm; m_actContactStatesOut.write(); m_COPInfo.tm = m_qRef.tm; @@ -667,12 +782,12 @@ void Stabilizer::calcFootOriginCoords (hrp::Vector3& foot_origin_pos, hrp::Matri leg_c[i].rot(0,1) = yv1(0); leg_c[i].rot(1,1) = yv1(1); leg_c[i].rot(2,1) = yv1(2); leg_c[i].rot(0,2) = ez(0); leg_c[i].rot(1,2) = ez(1); leg_c[i].rot(2,2) = ez(2); } - if (contact_states[contact_states_index_map["rleg"]] && - contact_states[contact_states_index_map["lleg"]]) { + if (ref_contact_states[contact_states_index_map["rleg"]] && + ref_contact_states[contact_states_index_map["lleg"]]) { rats::mid_coords(tmpc, 0.5, leg_c[0], leg_c[1]); foot_origin_pos = tmpc.pos; foot_origin_rot = tmpc.rot; - } else if (contact_states[contact_states_index_map["rleg"]]) { + } else if (ref_contact_states[contact_states_index_map["rleg"]]) { foot_origin_pos = leg_c[contact_states_index_map["rleg"]].pos; foot_origin_rot = leg_c[contact_states_index_map["rleg"]].rot; } else { @@ -686,7 +801,7 @@ void Stabilizer::getActualParameters () // Actual world frame => hrp::Vector3 foot_origin_pos; hrp::Matrix33 foot_origin_rot; - if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { + if (st_algorithm != OpenHRP::StabilizerService::TPCC) { // update by current joint angles for ( int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qCurrent.data[i]; @@ -714,7 +829,7 @@ void Stabilizer::getActualParameters () act_cog = m_robot->calcCM(); // zmp on_ground = false; - if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { + if (st_algorithm != OpenHRP::StabilizerService::TPCC) { on_ground = calcZMP(act_zmp, zmp_origin_off+foot_origin_pos(2)); } else { on_ground = calcZMP(act_zmp, ref_zmp(2)); @@ -722,32 +837,32 @@ void Stabilizer::getActualParameters () // set actual contact states for (size_t i = 0; i < stikp.size(); i++) { std::string limb_name = stikp[i].ee_name; - m_actContactStates.data[contact_states_index_map[limb_name]] = isContact(contact_states_index_map[limb_name]); + size_t idx = contact_states_index_map[limb_name]; + act_contact_states[idx] = isContact(idx); + m_actContactStates.data[idx] = act_contact_states[idx]; } // <= Actual world frame // convert absolute (in st) -> root-link relative rel_act_zmp = m_robot->rootLink()->R.transpose() * (act_zmp - m_robot->rootLink()->p); - if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { + if (st_algorithm != OpenHRP::StabilizerService::TPCC) { // Actual foot_origin frame => act_zmp = foot_origin_rot.transpose() * (act_zmp - foot_origin_pos); act_cog = foot_origin_rot.transpose() * (act_cog - foot_origin_pos); //act_cogvel = foot_origin_rot.transpose() * act_cogvel; - if (contact_states != prev_contact_states) { + if (ref_contact_states != prev_ref_contact_states) { act_cogvel = (foot_origin_rot.transpose() * prev_act_foot_origin_rot) * act_cogvel; } else { act_cogvel = (act_cog - prev_act_cog)/dt; } - prev_act_foot_origin_rot = foot_origin_rot; act_cogvel = act_cogvel_filter->passFilter(act_cogvel); prev_act_cog = act_cog; //act_root_rot = m_robot->rootLink()->R; for (size_t i = 0; i < stikp.size(); i++) { hrp::Link* target = m_robot->link(stikp[i].target_name); //hrp::Vector3 act_ee_p = target->p + target->R * stikp[i].localCOPPos; - hrp::Vector3 act_ee_p = target->p + target->R * stikp[i].localp; - //target_ee_R[i] = target->R * stikp[i].localR; - target_ee_diff_p[i] -= foot_origin_rot.transpose() * (act_ee_p - foot_origin_pos); + hrp::Vector3 _act_ee_p = target->p + target->R * stikp[i].localp; + act_ee_p[i] = foot_origin_rot.transpose() * (_act_ee_p - foot_origin_pos); act_ee_R[i] = foot_origin_rot.transpose() * (target->R * stikp[i].localR); } // capture point @@ -788,11 +903,15 @@ void Stabilizer::getActualParameters () std::vector ee_name; // distribute new ZMP into foot force & moment - std::vector ref_force, ref_moment; + std::vector tmp_ref_force, tmp_ref_moment; std::vector limb_gains; + std::vector ee_forcemoment_distribution_weight; + std::vector tmp_toeheel_ratio; if (control_mode == MODE_ST) { std::vector ee_pos, cop_pos; std::vector ee_rot; + std::vector is_contact_list; + is_contact_list.reserve(stikp.size()); for (size_t i = 0; i < stikp.size(); i++) { STIKParam& ikp = stikp[i]; if (!is_feedback_control_enable[i]) continue; @@ -802,12 +921,20 @@ void Stabilizer::getActualParameters () ee_rot.push_back(target->R * ikp.localR); ee_name.push_back(ikp.ee_name); limb_gains.push_back(ikp.swing_support_gain); - ref_force.push_back(hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2])); - ref_moment.push_back(hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5])); + tmp_ref_force.push_back(hrp::Vector3(foot_origin_rot * ref_force[i])); + tmp_ref_moment.push_back(hrp::Vector3(foot_origin_rot * ref_moment[i])); rel_ee_pos.push_back(foot_origin_rot.transpose() * (ee_pos.back() - foot_origin_pos)); rel_ee_rot.push_back(foot_origin_rot.transpose() * ee_rot.back()); rel_ee_name.push_back(ee_name.back()); + is_contact_list.push_back(act_contact_states[i]); + // std::cerr << ee_forcemoment_distribution_weight[i] << std::endl; + ee_forcemoment_distribution_weight.push_back(hrp::dvector6::Zero(6,1)); + for (size_t j = 0; j < 6; j++) { + ee_forcemoment_distribution_weight[i][j] = ikp.eefm_ee_forcemoment_distribution_weight[j]; + } + tmp_toeheel_ratio.push_back(toeheel_ratio[i]); } + // All state variables are foot_origin coords relative if (DEBUGP) { std::cerr << "[" << m_profile.instance_name << "] ee values" << std::endl; @@ -821,28 +948,44 @@ void Stabilizer::getActualParameters () } } + // truncate ZMP + if (use_zmp_truncation) { + Eigen::Vector2d tmp_new_refzmp(new_refzmp.head(2)); + szd->get_vertices(support_polygon_vetices); + szd->calc_convex_hull(support_polygon_vetices, ref_contact_states, ee_pos, ee_rot); + if (!szd->is_inside_support_polygon(tmp_new_refzmp, hrp::Vector3::Zero(), true, std::string(m_profile.instance_name))) new_refzmp.head(2) = tmp_new_refzmp; + } + // Distribute ZMP into each EE force/moment at each COP if (st_algorithm == OpenHRP::StabilizerService::EEFM) { // Modified version of distribution in Equation (4)-(6) and (10)-(13) in the paper [1]. - szd->distributeZMPToForceMoments(ref_force, ref_moment, - ee_pos, cop_pos, ee_rot, ee_name, limb_gains, + szd->distributeZMPToForceMoments(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), eefm_gravitational_acceleration * total_mass, dt, DEBUGP, std::string(m_profile.instance_name)); } else if (st_algorithm == OpenHRP::StabilizerService::EEFMQP) { - szd->distributeZMPToForceMomentsQP(ref_force, ref_moment, - ee_pos, cop_pos, ee_rot, ee_name, limb_gains, + szd->distributeZMPToForceMomentsQP(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), eefm_gravitational_acceleration * total_mass, dt, DEBUGP, std::string(m_profile.instance_name), (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP)); } else if (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { - szd->distributeZMPToForceMomentsPseudoInverse(ref_force, ref_moment, - ee_pos, cop_pos, ee_rot, ee_name, limb_gains, + szd->distributeZMPToForceMomentsPseudoInverse(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), eefm_gravitational_acceleration * total_mass, dt, DEBUGP, std::string(m_profile.instance_name), - (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP)); + (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP), is_contact_list); + } else if (st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP2) { + szd->distributeZMPToForceMomentsPseudoInverse2(tmp_ref_force, tmp_ref_moment, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, tmp_toeheel_ratio, + new_refzmp, hrp::Vector3(foot_origin_rot * ref_zmp + foot_origin_pos), + foot_origin_rot * ref_total_force, foot_origin_rot * ref_total_moment, + ee_forcemoment_distribution_weight, + eefm_gravitational_acceleration * total_mass, dt, + DEBUGP, std::string(m_profile.instance_name)); } // for debug output new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos); @@ -851,63 +994,102 @@ void Stabilizer::getActualParameters () // foor modif if (control_mode == MODE_ST) { hrp::Vector3 f_diff(hrp::Vector3::Zero()); + std::vector large_swing_f_diff(3, false); // moment control + act_total_foot_origin_moment = hrp::Vector3::Zero(); for (size_t i = 0; i < stikp.size(); i++) { STIKParam& ikp = stikp[i]; + std::vector large_swing_m_diff(3, false); if (!is_feedback_control_enable[i]) continue; hrp::Sensor* sensor = m_robot->sensor(ikp.sensor_name); hrp::Link* target = m_robot->link(ikp.target_name); // Convert moment at COP => moment at ee size_t idx = contact_states_index_map[ikp.ee_name]; - ikp.ref_moment = ref_moment[idx] + ((target->R * ikp.localCOPPos + target->p) - (target->R * ikp.localp + target->p)).cross(ref_force[idx]); - ikp.ref_force = ref_force[idx]; + ikp.ref_moment = tmp_ref_moment[idx] + ((target->R * ikp.localCOPPos + target->p) - (target->R * ikp.localp + target->p)).cross(tmp_ref_force[idx]); + ikp.ref_force = tmp_ref_force[idx]; // Actual world frame => hrp::Vector3 sensor_force = (sensor->link->R * sensor->localR) * hrp::Vector3(m_wrenches[i].data[0], m_wrenches[i].data[1], m_wrenches[i].data[2]); hrp::Vector3 sensor_moment = (sensor->link->R * sensor->localR) * hrp::Vector3(m_wrenches[i].data[3], m_wrenches[i].data[4], m_wrenches[i].data[5]); //hrp::Vector3 ee_moment = ((sensor->link->R * sensor->localPos + sensor->link->p) - (target->R * ikp.localCOPPos + target->p)).cross(sensor_force) + sensor_moment; hrp::Vector3 ee_moment = ((sensor->link->R * sensor->localPos + sensor->link->p) - (target->R * ikp.localp + target->p)).cross(sensor_force) + sensor_moment; // <= Actual world frame - // Convert force & moment as foot origin coords relative - ikp.ref_moment = foot_origin_rot.transpose() * ikp.ref_moment; - ikp.ref_force = foot_origin_rot.transpose() * ikp.ref_force; - sensor_force = foot_origin_rot.transpose() * sensor_force; - ee_moment = foot_origin_rot.transpose() * ee_moment; + hrp::Matrix33 ee_R(target->R * ikp.localR); + // Actual ee frame => + ikp.ref_moment = ee_R.transpose() * ikp.ref_moment; + ikp.ref_force = ee_R.transpose() * ikp.ref_force; + sensor_force = ee_R.transpose() * sensor_force; + ee_moment = ee_R.transpose() * ee_moment; if ( i == 0 ) f_diff += -1*sensor_force; else f_diff += sensor_force; + for (size_t j = 0; j < 3; ++j) { + if ((!ref_contact_states[i] || !act_contact_states[i]) && fabs(ikp.ref_force(j) - sensor_force(j)) > eefm_swing_damping_force_thre[j]) large_swing_f_diff[j] = true; + if ((!ref_contact_states[i] || !act_contact_states[i]) && (fabs(ikp.ref_moment(j) - ee_moment(j)) > eefm_swing_damping_moment_thre[j])) large_swing_m_diff[j] = true; + } // Moment limitation - hrp::Matrix33 ee_R(target->R * ikp.localR); ikp.ref_moment = ee_R * vlimit((ee_R.transpose() * ikp.ref_moment), ikp.eefm_ee_moment_limit); // calcDampingControl - // d_foot_rpy and d_foot_pos is (actual) foot origin coords relative value because these use foot origin coords relative force & moment + // ee_d_foot_rpy and ee_d_foot_pos is (actual) end effector coords relative value because these use end effector coords relative force & moment { // Rot // Basically Equation (16) and (17) in the paper [1] - hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * ikp.eefm_rot_damping_gain * 10 + transition_smooth_gain * ikp.eefm_rot_damping_gain; - ikp.d_foot_rpy = calcDampingControl(ikp.ref_moment, ee_moment, ikp.d_foot_rpy, tmp_damping_gain, ikp.eefm_rot_time_const); - ikp.d_foot_rpy = vlimit(ikp.d_foot_rpy, -1 * ikp.eefm_rot_compensation_limit, ikp.eefm_rot_compensation_limit); + hrp::Vector3 tmp_damping_gain; + for (size_t j = 0; j < 3; ++j) { + if (!eefm_use_swing_damping || !large_swing_m_diff[j]) tmp_damping_gain(j) = (1-transition_smooth_gain) * ikp.eefm_rot_damping_gain(j) * 10 + transition_smooth_gain * ikp.eefm_rot_damping_gain(j); + else tmp_damping_gain(j) = (1-transition_smooth_gain) * eefm_swing_rot_damping_gain(j) * 10 + transition_smooth_gain * eefm_swing_rot_damping_gain(j); + } + ikp.ee_d_foot_rpy = calcDampingControl(ikp.ref_moment, ee_moment, ikp.ee_d_foot_rpy, tmp_damping_gain, ikp.eefm_rot_time_const); + ikp.ee_d_foot_rpy = vlimit(ikp.ee_d_foot_rpy, -1 * ikp.eefm_rot_compensation_limit, ikp.eefm_rot_compensation_limit); } if (!eefm_use_force_difference_control) { // Pos hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * ikp.eefm_pos_damping_gain * 10 + transition_smooth_gain * ikp.eefm_pos_damping_gain; - ikp.d_foot_pos = calcDampingControl(ikp.ref_force, sensor_force, ikp.d_foot_pos, tmp_damping_gain, ikp.eefm_pos_time_const_support); - ikp.d_foot_pos = vlimit(ikp.d_foot_pos, -1 * ikp.eefm_pos_compensation_limit, ikp.eefm_pos_compensation_limit); + ikp.ee_d_foot_pos = calcDampingControl(ikp.ref_force, sensor_force, ikp.ee_d_foot_pos, tmp_damping_gain, ikp.eefm_pos_time_const_support); + ikp.ee_d_foot_pos = vlimit(ikp.ee_d_foot_pos, -1 * ikp.eefm_pos_compensation_limit, ikp.eefm_pos_compensation_limit); } - // Actual ee frame => - ikp.ee_d_foot_rpy = ee_R.transpose() * (foot_origin_rot * ikp.d_foot_rpy); + // Convert force & moment as foot origin coords relative + ikp.ref_moment = foot_origin_rot.transpose() * ee_R * ikp.ref_moment; + ikp.ref_force = foot_origin_rot.transpose() * ee_R * ikp.ref_force; + sensor_force = foot_origin_rot.transpose() * ee_R * sensor_force; + ee_moment = foot_origin_rot.transpose() * ee_R * ee_moment; + ikp.d_foot_rpy = foot_origin_rot.transpose() * ee_R * ikp.ee_d_foot_rpy; + ikp.d_foot_pos = foot_origin_rot.transpose() * ee_R * ikp.ee_d_foot_pos; + // tilt Check : only flat plane is supported + { + hrp::Vector3 plane_x = target_ee_R[i].col(0); + hrp::Vector3 plane_y = target_ee_R[i].col(1); + hrp::Matrix33 act_ee_R_world = target->R * stikp[i].localR; + hrp::Vector3 normal_vector = act_ee_R_world.col(2); + /* projected_normal = c1 * plane_x + c2 * plane_y : c1 = plane_x.dot(normal_vector), c2 = plane_y.dot(normal_vector) because (normal-vector - projected_normal) is orthogonal to plane */ + projected_normal.at(i) = plane_x.dot(normal_vector) * plane_x + plane_y.dot(normal_vector) * plane_y; + act_force.at(i) = sensor_force; + } + //act_total_foot_origin_moment += (target->R * ikp.localCOPPos + target->p).cross(sensor_force) + ee_moment; + act_total_foot_origin_moment += (target->R * ikp.localp + target->p - foot_origin_pos).cross(sensor_force) + ee_moment; } + act_total_foot_origin_moment = foot_origin_rot.transpose() * act_total_foot_origin_moment; if (eefm_use_force_difference_control) { // fxyz control // foot force difference control version // Basically Equation (18) in the paper [1] hrp::Vector3 ref_f_diff = (stikp[1].ref_force-stikp[0].ref_force); - if ( (contact_states[contact_states_index_map["rleg"]] && contact_states[contact_states_index_map["lleg"]]) // Reference : double support phase - || (isContact(0) && isContact(1)) ) { // Actual : double support phase + if (ref_contact_states != prev_ref_contact_states) pos_ctrl = (foot_origin_rot.transpose() * prev_act_foot_origin_rot) * pos_ctrl; + if (eefm_use_swing_damping) { + hrp::Vector3 tmp_damping_gain; + for (size_t i = 0; i < 3; ++i) { + if (!large_swing_f_diff[i]) tmp_damping_gain(i) = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain(i) * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain(i); + else tmp_damping_gain(i) = (1-transition_smooth_gain) * eefm_swing_pos_damping_gain(i) * 10 + transition_smooth_gain * eefm_swing_pos_damping_gain(i); + } + pos_ctrl = calcDampingControl (ref_f_diff, f_diff, pos_ctrl, + tmp_damping_gain, stikp[0].eefm_pos_time_const_support); + } else { + if ( (ref_contact_states[contact_states_index_map["rleg"]] && ref_contact_states[contact_states_index_map["lleg"]]) // Reference : double support phase + || (act_contact_states[0] && act_contact_states[1]) ) { // Actual : double support phase // Temporarily use first pos damping gain (stikp[0]) hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain; pos_ctrl = calcDampingControl (ref_f_diff, f_diff, pos_ctrl, tmp_damping_gain, stikp[0].eefm_pos_time_const_support); - } else { + } else { double remain_swing_time; - if ( !contact_states[contact_states_index_map["rleg"]] ) { // rleg swing + if ( !ref_contact_states[contact_states_index_map["rleg"]] ) { // rleg swing remain_swing_time = m_controlSwingSupportTime.data[contact_states_index_map["rleg"]]; } else { // lleg swing remain_swing_time = m_controlSwingSupportTime.data[contact_states_index_map["lleg"]]; @@ -918,6 +1100,7 @@ void Stabilizer::getActualParameters () hrp::Vector3 tmp_damping_gain = (1-transition_smooth_gain) * stikp[0].eefm_pos_damping_gain * 10 + transition_smooth_gain * stikp[0].eefm_pos_damping_gain; hrp::Vector3 tmp_time_const = (1-tmp_ratio)*eefm_pos_time_const_swing*hrp::Vector3::Ones()+tmp_ratio*stikp[0].eefm_pos_time_const_support; pos_ctrl = calcDampingControl (tmp_ratio * ref_f_diff, tmp_ratio * f_diff, pos_ctrl, tmp_damping_gain, tmp_time_const); + } } // zctrl = vlimit(zctrl, -0.02, 0.02); // Temporarily use first pos compensation limit (stikp[0]) @@ -934,8 +1117,7 @@ void Stabilizer::getActualParameters () } for (size_t i = 0; i < ee_name.size(); i++) { std::cerr << "[" << m_profile.instance_name << "] " - << "d_foot_pos (" << ee_name[i] << ") = [" << stikp[i].d_foot_pos(0)*1e3 << " " << stikp[i].d_foot_pos(1)*1e3 << " " << stikp[i].d_foot_pos(2)*1e3 << "] [mm]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] " + << "d_foot_pos (" << ee_name[i] << ") = [" << stikp[i].d_foot_pos(0)*1e3 << " " << stikp[i].d_foot_pos(1)*1e3 << " " << stikp[i].d_foot_pos(2)*1e3 << "] [mm], " << "d_foot_rpy (" << ee_name[i] << ") = [" << stikp[i].d_foot_rpy(0)*180.0/M_PI << " " << stikp[i].d_foot_rpy(1)*180.0/M_PI << " " << stikp[i].d_foot_rpy(2)*180.0/M_PI << "] [deg]" << std::endl; } } @@ -945,6 +1127,7 @@ void Stabilizer::getActualParameters () // fz[i], f_zctrl[i], eefm_pos_damping_gain, eefm_pos_time_const); // f_zctrl[i] = vlimit(f_zctrl[i], -0.05, 0.05); // } + calcDiffFootOriginExtMoment (); } } // st_algorithm == OpenHRP::StabilizerService::EEFM @@ -967,7 +1150,9 @@ void Stabilizer::getActualParameters () m_robot->rootLink()->R = current_root_R; m_robot->calcForwardKinematics(); } - copy (contact_states.begin(), contact_states.end(), prev_contact_states.begin()); + copy (ref_contact_states.begin(), ref_contact_states.end(), prev_ref_contact_states.begin()); + if (control_mode != MODE_ST) d_pos_z_root = 0.0; + prev_act_foot_origin_rot = foot_origin_rot; } void Stabilizer::getTargetParameters () @@ -977,7 +1162,7 @@ void Stabilizer::getTargetParameters () if ( transition_count == 0 ) { transition_smooth_gain = 1.0; } else { - double max_transition_count = transition_time / dt; + double max_transition_count = calcMaxTransitionCount(); transition_smooth_gain = 1/(1+exp(-9.19*(((max_transition_count - std::fabs(transition_count)) / max_transition_count) - 0.5))); } if (transition_count > 0) { @@ -993,7 +1178,7 @@ void Stabilizer::getTargetParameters () transition_count++; } else if ( transition_count > 0 ) { if ( transition_count == 1 ) { - std::cerr << "[" << m_profile.instance_name << "] Move to MODE_IDLE" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm << "] Move to MODE_IDLE" << std::endl; reset_emergency_flag = true; } transition_count--; @@ -1007,26 +1192,49 @@ void Stabilizer::getTargetParameters () m_robot->rootLink()->R = target_root_R; m_robot->calcForwardKinematics(); ref_zmp = m_robot->rootLink()->R * hrp::Vector3(m_zmpRef.data.x, m_zmpRef.data.y, m_zmpRef.data.z) + m_robot->rootLink()->p; // base frame -> world frame - if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { + hrp::Vector3 foot_origin_pos; + hrp::Matrix33 foot_origin_rot; + calcFootOriginCoords (foot_origin_pos, foot_origin_rot); + if (st_algorithm != OpenHRP::StabilizerService::TPCC) { // apply inverse system hrp::Vector3 tmp_ref_zmp = ref_zmp + eefm_zmp_delay_time_const[0] * (ref_zmp - prev_ref_zmp) / dt; prev_ref_zmp = ref_zmp; ref_zmp = tmp_ref_zmp; } ref_cog = m_robot->calcCM(); + ref_total_force = hrp::Vector3::Zero(); + ref_total_moment = hrp::Vector3::Zero(); // Total moment around reference ZMP tmp + ref_total_foot_origin_moment = hrp::Vector3::Zero(); for (size_t i = 0; i < stikp.size(); i++) { hrp::Link* target = m_robot->link(stikp[i].target_name); //target_ee_p[i] = target->p + target->R * stikp[i].localCOPPos; target_ee_p[i] = target->p + target->R * stikp[i].localp; target_ee_R[i] = target->R * stikp[i].localR; + ref_force[i] = hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2]); + ref_moment[i] = hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5]); + ref_total_force += ref_force[i]; +#ifdef FORCE_MOMENT_DIFF_CONTROL + // Force/moment diff control + ref_total_moment += (target_ee_p[i]-ref_zmp).cross(ref_force[i]); +#else + // Force/moment control + ref_total_moment += (target_ee_p[i]-ref_zmp).cross(hrp::Vector3(m_ref_wrenches[i].data[0], m_ref_wrenches[i].data[1], m_ref_wrenches[i].data[2])) + + hrp::Vector3(m_ref_wrenches[i].data[3], m_ref_wrenches[i].data[4], m_ref_wrenches[i].data[5]); +#endif + if (is_feedback_control_enable[i]) { + ref_total_foot_origin_moment += (target_ee_p[i]-foot_origin_pos).cross(ref_force[i]) + ref_moment[i]; + } } // <= Reference world frame - if (st_algorithm == OpenHRP::StabilizerService::EEFM || st_algorithm == OpenHRP::StabilizerService::EEFMQP || st_algorithm == OpenHRP::StabilizerService::EEFMQPCOP) { + // Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST) because the coordinates for ref_cog differs among st algorithms. + if (transition_count == (-1 * calcMaxTransitionCount() + 1)) { // max transition count. In MODE_IDLE => MODE_ST, transition_count is < 0 and upcounter. "+ 1" is upcount at the beginning of this function. + prev_ref_cog = ref_cog; + std::cerr << "[" << m_profile.instance_name << "] Reset prev_ref_cog for transition (MODE_IDLE=>MODE_ST)." << std::endl; + } + + if (st_algorithm != OpenHRP::StabilizerService::TPCC) { // Reference foot_origin frame => - hrp::Vector3 foot_origin_pos; - hrp::Matrix33 foot_origin_rot; - calcFootOriginCoords (foot_origin_pos, foot_origin_rot); // initialize for new_refzmp new_refzmp = ref_zmp; rel_cog = m_robot->rootLink()->R.transpose() * (ref_cog-m_robot->rootLink()->p); @@ -1035,21 +1243,27 @@ void Stabilizer::getTargetParameters () ref_zmp = foot_origin_rot.transpose() * (ref_zmp - foot_origin_pos); ref_cog = foot_origin_rot.transpose() * (ref_cog - foot_origin_pos); new_refzmp = foot_origin_rot.transpose() * (new_refzmp - foot_origin_pos); - if (contact_states != prev_contact_states) { + if (ref_contact_states != prev_ref_contact_states) { ref_cogvel = (foot_origin_rot.transpose() * prev_ref_foot_origin_rot) * ref_cogvel; } else { ref_cogvel = (ref_cog - prev_ref_cog)/dt; } - prev_ref_foot_origin_rot = foot_origin_rot; + prev_ref_foot_origin_rot = ref_foot_origin_rot = foot_origin_rot; for (size_t i = 0; i < stikp.size(); i++) { - //target_ee_diff_p[i] += foot_origin_rot.transpose() * (target_ee_p[i] - foot_origin_pos); - target_ee_diff_p[i] = foot_origin_rot.transpose() * (target_ee_p[i] - foot_origin_pos); + stikp[i].target_ee_diff_p = foot_origin_rot.transpose() * (target_ee_p[i] - foot_origin_pos); + stikp[i].target_ee_diff_r = foot_origin_rot.transpose() * target_ee_R[i]; + ref_force[i] = foot_origin_rot.transpose() * ref_force[i]; + ref_moment[i] = foot_origin_rot.transpose() * ref_moment[i]; } + ref_total_foot_origin_moment = foot_origin_rot.transpose() * ref_total_foot_origin_moment; + ref_total_force = foot_origin_rot.transpose() * ref_total_force; + ref_total_moment = foot_origin_rot.transpose() * ref_total_moment; target_foot_origin_rot = foot_origin_rot; // capture point ref_cp = ref_cog + ref_cogvel / std::sqrt(eefm_gravitational_acceleration / (ref_cog - ref_zmp)(2)); rel_ref_cp = hrp::Vector3(ref_cp(0), ref_cp(1), ref_zmp(2)); rel_ref_cp = m_robot->rootLink()->R.transpose() * ((foot_origin_pos + foot_origin_rot * rel_ref_cp) - m_robot->rootLink()->p); + sbp_cog_offset = foot_origin_rot.transpose() * sbp_cog_offset; // <= Reference foot_origin frame } else { ref_cogvel = (ref_cog - prev_ref_cog)/dt; @@ -1136,32 +1350,67 @@ void Stabilizer::calcStateForEmergencySignal() // CP Check bool is_cp_outside = false; if (on_ground && transition_count == 0 && control_mode == MODE_ST) { - SimpleZMPDistributor::leg_type support_leg; - size_t l_idx, r_idx; - Eigen::Vector2d tmp_cp; - for (size_t i = 0; i < rel_ee_name.size(); i++) { - if (rel_ee_name[i]=="rleg") r_idx = i; - else if (rel_ee_name[i]=="lleg") l_idx = i; - } - for (size_t i = 0; i < 2; i++) { - tmp_cp(i) = act_cp(i); - } - if (isContact(contact_states_index_map["rleg"]) && isContact(contact_states_index_map["lleg"])) support_leg = SimpleZMPDistributor::BOTH; - else if (isContact(contact_states_index_map["rleg"])) support_leg = SimpleZMPDistributor::RLEG; - else if (isContact(contact_states_index_map["lleg"])) support_leg = SimpleZMPDistributor::LLEG; - if (!is_walking || is_estop_while_walking) is_cp_outside = !szd->is_inside_support_polygon(tmp_cp, rel_ee_pos, rel_ee_rot, rel_ee_name, support_leg, cp_check_margin, - sbp_cog_offset); + Eigen::Vector2d tmp_cp = act_cp.head(2); + szd->get_margined_vertices(margined_support_polygon_vetices); + szd->calc_convex_hull(margined_support_polygon_vetices, act_contact_states, rel_ee_pos, rel_ee_rot); + if (!is_walking || is_estop_while_walking) is_cp_outside = !szd->is_inside_support_polygon(tmp_cp, - sbp_cog_offset); if (DEBUGP) { - std::cerr << "[" << m_profile.instance_name << "] CP value " << "[" << act_cp(0) << "," << act_cp(1) << "] [m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] CP value " << "[" << act_cp(0) << "," << act_cp(1) << "] [m], " + << "sbp cog offset [" << sbp_cog_offset(0) << " " << sbp_cog_offset(1) << "], outside ? " + << (is_cp_outside?"Outside":"Inside") + << std::endl; } if (is_cp_outside) { if (initial_cp_too_large_error || loop % static_cast (0.2/dt) == 0 ) { // once per 0.2[s] - std::cerr << "[" << m_profile.instance_name << "] CP too large error " << "[" << act_cp(0) << "," << act_cp(1) << "] [m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] CP too large error " << "[" << act_cp(0) << "," << act_cp(1) << "] [m]" << std::endl; } initial_cp_too_large_error = false; } else { initial_cp_too_large_error = true; } } + // tilt Check + hrp::Vector3 fall_direction = hrp::Vector3::Zero(); + bool is_falling = false, will_fall = false; + { + double total_force = 0.0; + for (size_t i = 0; i < stikp.size(); i++) { + if (is_zmp_calc_enable[i]) { + if (is_walking) { + if (projected_normal.at(i).norm() > sin(tilt_margin[0])) { + will_fall = true; + if (m_will_fall_counter[i] % static_cast (1.0/dt) == 0 ) { // once per 1.0[s] + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] " << stikp[i].ee_name << " cannot support total weight, " + << "swgsuptime : " << m_controlSwingSupportTime.data[i] << ", state : " << ref_contact_states[i] + << ", otherwise robot will fall down toward " << "(" << projected_normal.at(i)(0) << "," << projected_normal.at(i)(1) << ") direction" << std::endl; + } + m_will_fall_counter[i]++; + } else { + m_will_fall_counter[i] = 0; + } + } + fall_direction += projected_normal.at(i) * act_force.at(i).norm(); + total_force += act_force.at(i).norm(); + } + } + if (on_ground && transition_count == 0 && control_mode == MODE_ST) { + fall_direction = fall_direction / total_force; + } else { + fall_direction = hrp::Vector3::Zero(); + } + if (fall_direction.norm() > sin(tilt_margin[1])) { + is_falling = true; + if (m_is_falling_counter % static_cast (0.2/dt) == 0) { // once per 0.2[s] + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] robot is falling down toward " << "(" << fall_direction(0) << "," << fall_direction(1) << ") direction" << std::endl; + } + m_is_falling_counter++; + } else { + m_is_falling_counter = 0; + } + } // Total check for emergency signal switch (emergency_check_mode) { case OpenHRP::StabilizerService::NO_CHECK: @@ -1173,6 +1422,9 @@ void Stabilizer::calcStateForEmergencySignal() case OpenHRP::StabilizerService::CP: is_emergency = is_cp_outside; break; + case OpenHRP::StabilizerService::TILT: + is_emergency = will_fall || is_falling; + break; default: break; } @@ -1191,8 +1443,11 @@ void Stabilizer::moveBasePosRotForBodyRPYControl () // Body rpy control // Basically Equation (1) and (2) in the paper [1] hrp::Vector3 ref_root_rpy = hrp::rpyFromRot(target_root_R); + bool is_root_rot_limit = false; for (size_t i = 0; i < 2; i++) { d_rpy[i] = transition_smooth_gain * (eefm_body_attitude_control_gain[i] * (ref_root_rpy(i) - act_base_rpy(i)) - 1/eefm_body_attitude_control_time_const[i] * d_rpy[i]) * dt + d_rpy[i]; + d_rpy[i] = vlimit(d_rpy[i], -1 * root_rot_compensation_limit[i], root_rot_compensation_limit[i]); + is_root_rot_limit = is_root_rot_limit || (std::fabs(std::fabs(d_rpy[i]) - root_rot_compensation_limit[i] ) < 1e-5); // near the limit } rats::rotm3times(current_root_R, target_root_R, hrp::rotFromRpy(d_rpy[0], d_rpy[1], 0)); m_robot->rootLink()->R = current_root_R; @@ -1200,14 +1455,26 @@ void Stabilizer::moveBasePosRotForBodyRPYControl () m_robot->calcForwardKinematics(); current_base_rpy = hrp::rpyFromRot(m_robot->rootLink()->R); current_base_pos = m_robot->rootLink()->p; + if ( DEBUGP || (is_root_rot_limit && loop%200==0) ) { + std::cerr << "[" << m_profile.instance_name << "] Root rot control" << std::endl; + if (is_root_rot_limit) std::cerr << "[" << m_profile.instance_name << "] Root rot limit reached!!" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] ref = [" << rad2deg(ref_root_rpy(0)) << " " << rad2deg(ref_root_rpy(1)) << "], " + << "act = [" << rad2deg(act_base_rpy(0)) << " " << rad2deg(act_base_rpy(1)) << "], " + << "cur = [" << rad2deg(current_base_rpy(0)) << " " << rad2deg(current_base_rpy(1)) << "], " + << "limit = [" << rad2deg(root_rot_compensation_limit[0]) << " " << rad2deg(root_rot_compensation_limit[1]) << "][deg]" << std::endl; + } }; void Stabilizer::calcSwingSupportLimbGain () { for (size_t i = 0; i < stikp.size(); i++) { STIKParam& ikp = stikp[i]; - if (contact_states[i]) { // Support - ikp.support_time += dt; + if (ref_contact_states[i]) { // Support + // Limit too large support time increment. Max time is 3600.0[s] = 1[h], this assumes that robot's one step time is smaller than 1[h]. + ikp.support_time = std::min(3600.0, ikp.support_time+dt); + // In some PC, does not work because the first line is optimized out. + // ikp.support_time += dt; + // ikp.support_time = std::min(3600.0, ikp.support_time); if (ikp.support_time > eefm_pos_transition_time) { ikp.swing_support_gain = (m_controlSwingSupportTime.data[i] / eefm_pos_transition_time); } else { @@ -1222,10 +1489,14 @@ void Stabilizer::calcSwingSupportLimbGain () if (DEBUGP) { std::cerr << "[" << m_profile.instance_name << "] SwingSupportLimbGain = ["; for (size_t i = 0; i < stikp.size(); i++) std::cerr << stikp[i].swing_support_gain << " "; - std::cerr << "], contact_states = ["; - for (size_t i = 0; i < stikp.size(); i++) std::cerr << contact_states[i] << " "; + std::cerr << "], ref_contact_states = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << ref_contact_states[i] << " "; std::cerr << "], sstime = ["; for (size_t i = 0; i < stikp.size(); i++) std::cerr << m_controlSwingSupportTime.data[i] << " "; + std::cerr << "], toeheel_ratio = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << toeheel_ratio[i] << " "; + std::cerr << "], support_time = ["; + for (size_t i = 0; i < stikp.size(); i++) std::cerr << stikp[i].support_time << " "; std::cerr << "]" << std::endl; } } @@ -1255,7 +1526,11 @@ void Stabilizer::calcTPCC() { // solveIK // IK target is link origin pos and rot, not ee pos and rot. //for (size_t jj = 0; jj < 5; jj++) { - for (size_t jj = 0; jj < 3; jj++) { + size_t max_ik_loop_count = 0; + for (size_t i = 0; i < stikp.size(); i++) { + if (max_ik_loop_count < stikp[i].ik_loop_count) max_ik_loop_count = stikp[i].ik_loop_count; + } + for (size_t jj = 0; jj < max_ik_loop_count; jj++) { hrp::Vector3 tmpcm = m_robot->calcCM(); for (size_t i = 0; i < 2; i++) { m_robot->rootLink()->p(i) = m_robot->rootLink()->p(i) + 0.9 * (newcog(i) - tmpcm(i)); @@ -1297,6 +1572,36 @@ void Stabilizer::calcEEForceMomentControl() { } } + // State calculation for swing ee compensation + // joint angle : current control output + // root pos : target root p + // root rot : actual root rot + { + // Calc status + m_robot->rootLink()->R = target_root_R; + m_robot->rootLink()->p = target_root_p; + m_robot->calcForwardKinematics(); + hrp::Sensor* sen = m_robot->sensor("gyrometer"); + hrp::Matrix33 senR = sen->link->R * sen->localR; + hrp::Matrix33 act_Rs(hrp::rotFromRpy(m_rpy.data.r, m_rpy.data.p, m_rpy.data.y)); + m_robot->rootLink()->R = act_Rs * (senR.transpose() * m_robot->rootLink()->R); + m_robot->calcForwardKinematics(); + hrp::Vector3 foot_origin_pos; + hrp::Matrix33 foot_origin_rot; + calcFootOriginCoords (foot_origin_pos, foot_origin_rot); + // Calculate foot_origin_coords-relative ee pos and rot + // Subtract them from target_ee_diff_xx + for (size_t i = 0; i < stikp.size(); i++) { + hrp::Link* target = m_robot->link(stikp[i].target_name); + stikp[i].target_ee_diff_p -= foot_origin_rot.transpose() * (target->p + target->R * stikp[i].localp - foot_origin_pos); + stikp[i].target_ee_diff_r = (foot_origin_rot.transpose() * target->R * stikp[i].localR).transpose() * stikp[i].target_ee_diff_r; + } + } + + // State calculation for control : calculate "current" state + // joint angle : current control output + // root pos : target + keep COG against rpy control + // root rot : target + rpy control moveBasePosRotForBodyRPYControl (); // Convert d_foot_pos in foot origin frame => "current" world frame @@ -1307,108 +1612,118 @@ void Stabilizer::calcEEForceMomentControl() { for (size_t i = 0; i < stikp.size(); i++) current_d_foot_pos.push_back(foot_origin_rot * stikp[i].d_foot_pos); - // Feet and hands modification - std::vector tmpp_list; // modified ee Pos - std::vector tmpR_list; // modified ee Rot -#define deg2rad(x) ((x) * M_PI / 180.0) + // Swing ee compensation. + calcSwingEEModification(); + + // solveIK + // IK target is link origin pos and rot, not ee pos and rot. + std::vector tmpp(stikp.size()); + std::vector tmpR(stikp.size()); + double tmp_d_pos_z_root = 0.0; for (size_t i = 0; i < stikp.size(); i++) { - hrp::Vector3 tmpp; // modified ee Pos - hrp::Matrix33 tmpR; // modified ee Rot + if (is_ik_enable[i]) { + // Add damping_control compensation to target value if (is_feedback_control_enable[i]) { - // moment control - rats::rotm3times(tmpR, target_ee_R[i], hrp::rotFromRpy(-stikp[i].ee_d_foot_rpy(0), -stikp[i].ee_d_foot_rpy(1), 0)); - // total_target_foot_p[i](0) = target_foot_p[i](0); - // total_target_foot_p[i](1) = target_foot_p[i](1); - // foot force difference control version - // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl; - // foot force independent damping control - tmpp = target_ee_p[i] - current_d_foot_pos[i]; + rats::rotm3times(tmpR[i], target_ee_R[i], hrp::rotFromRpy(-1*stikp[i].ee_d_foot_rpy)); + // foot force difference control version + // total_target_foot_p[i](2) = target_foot_p[i](2) + (i==0?0.5:-0.5)*zctrl; + // foot force independent damping control + tmpp[i] = target_ee_p[i] - current_d_foot_pos[i]; } else { - target_ee_diff_p[i] *= transition_smooth_gain; - tmpp = target_ee_p[i] + eefm_ee_pos_error_p_gain * target_foot_origin_rot * target_ee_diff_p_filter[i]->passFilter(target_ee_diff_p[i]);// tempolarily disabled - tmpR = target_ee_R[i]; + tmpp[i] = target_ee_p[i]; + tmpR[i] = target_ee_R[i]; } - tmpR_list.push_back(tmpR); - tmpp_list.push_back(tmpp); + // Add swing ee compensation + rats::rotm3times(tmpR[i], tmpR[i], hrp::rotFromRpy(stikp[i].d_rpy_swing)); + tmpp[i] = tmpp[i] + foot_origin_rot * stikp[i].d_pos_swing; + } } - // follow swing foot rotation to target one in single support phase on the assumption that the robot rotates around support foot. - { - hrp::Matrix33 cur_sup_R, act_sup_R; - hrp::Vector3 cur_sup_p; - for (size_t i = 0; i < stikp.size(); i++) { - if (isContact(i)) { - cur_sup_R = tmpR_list.at(i); - cur_sup_p = tmpp_list.at(i); - act_sup_R = act_ee_R.at(i); - break; - } - } - for (size_t i = 0; i < stikp.size(); i++) { - if (isContact(i) or contact_states[contact_states_index_map[stikp[i].ee_name]]) { - /* method A */ - // for (size_t j = 0; j < 3; j++) { - // d_rpy_swing.at(i)[j] = (-1 / stikp[i].eefm_swing_rot_time_const[j] * d_rpy_swing.at(i)[j]) * dt + d_rpy_swing.at(i)[j]; - // d_pos_swing.at(i)[j] = (-1 / stikp[i].eefm_swing_pos_time_const[j] * d_pos_swing.at(i)[j]) * dt + d_pos_swing.at(i)[j]; - // } - /* method B */ - d_rpy_swing.at(i) = hrp::Vector3::Zero(); - d_pos_swing.at(i) = hrp::Vector3::Zero(); - } else { - /* rotation */ - { - hrp::Matrix33 cur_swg_R = tmpR_list.at(i); - hrp::Matrix33 swg_R_relative_to_sup_R = cur_sup_R.transpose() * cur_swg_R; - hrp::Matrix33 new_swg_R; - rats::rotm3times(new_swg_R, act_sup_R, swg_R_relative_to_sup_R); - rats::rotm3times(new_swg_R, foot_origin_rot, new_swg_R); - hrp::Vector3 tmp_diff_rpy = hrp::rpyFromRot(new_swg_R.transpose() * tmpR_list.at(i)); - for (size_t j = 0; j < 3; j++) { - /* method A */ - // d_rpy_swing.at(i)[j] = (stikp[i].eefm_swing_rot_spring_gain[j] * tmp_diff_rpy[j] - 1 / stikp[i].eefm_swing_rot_time_const[j] * d_rpy_swing.at(i)[j]) * dt + d_rpy_swing.at(i)[j]; - /* method B */ - d_rpy_swing.at(i)[j] = tmp_diff_rpy[j] * stikp[i].eefm_swing_rot_spring_gain[j]; - } - rats::rotm3times(tmpR_list.at(i), tmpR_list.at(i), hrp::rotFromRpy(d_rpy_swing.at(i))); - } - /* position */ - { - hrp::Vector3 cur_swg_p = tmpp_list.at(i); - hrp::Vector3 swg_p_relative_to_sup_R = cur_sup_R.transpose() * (cur_swg_p - cur_sup_p); - hrp::Vector3 new_swg_p = (foot_origin_rot * act_sup_R) * swg_p_relative_to_sup_R + cur_sup_p; - hrp::Vector3 tmp_diff_pos = tmpp_list.at(i) - new_swg_p; - for (size_t j = 0; j < 3; j++) { - /* method A */ - // d_pos_swing.at(i)[j] = (stikp[i].eefm_swing_pos_spring_gain[j] * tmp_diff_pos[j] - 1 / stikp[i].eefm_swing_pos_time_const[j] * d_pos_swing.at(i)[j]) * dt + d_pos_swing.at(i)[j]; - /* method B */ - d_pos_swing.at(i)[j] = tmp_diff_pos[j] * stikp[i].eefm_swing_pos_spring_gain[j]; - } - tmpp_list.at(i) = tmpp_list.at(i) + d_pos_swing.at(i); - } - } - } - if (DEBUGP) { - for (size_t i = 0; i < stikp.size(); i++) { - if (is_feedback_control_enable[i]) { - std::cerr << "[" << m_profile.instance_name << "] " - << "d_rpy_swing (" << stikp[i].ee_name << ") = " << (d_rpy_swing.at(i) / M_PI * 180.0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[deg]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] " - << "d_pos_swing (" << stikp[i].ee_name << ") = " << (d_pos_swing.at(i) * 1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl; - } - } + + limbStretchAvoidanceControl(tmpp ,tmpR); + + // IK + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + for (size_t jj = 0; jj < stikp[i].ik_loop_count; jj++) { + jpe_v[i]->calcInverseKinematics2Loop(tmpp[i], tmpR[i], 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain, + //stikp[i].localCOPPos; + stikp[i].localp, + stikp[i].localR); } + } } - // solveIK - // IK target is link origin pos and rot, not ee pos and rot. - for (size_t jj = 0; jj < 3; jj++) { +} + +// Swing ee compensation. +// Calculate compensation values to minimize the difference between "current" foot-origin-coords-relative pos and rot and "target" foot-origin-coords-relative pos and rot for swing ee. +// Input : target_ee_diff_p, target_ee_diff_r +// Output : d_pos_swing, d_rpy_swing +void Stabilizer::calcSwingEEModification () +{ + for (size_t i = 0; i < stikp.size(); i++) { + // Calc compensation values + double limit_pos = 30 * 1e-3; // 30[mm] limit + double limit_rot = deg2rad(10); // 10[deg] limit + if (ref_contact_states[contact_states_index_map[stikp[i].ee_name]] || act_contact_states[contact_states_index_map[stikp[i].ee_name]]) { + // If actual contact or target contact is ON, do not use swing ee compensation. Exponential zero retrieving. + stikp[i].d_rpy_swing = calcDampingControl(stikp[i].d_rpy_swing, stikp[i].eefm_swing_rot_time_const); + stikp[i].d_pos_swing = calcDampingControl(stikp[i].d_pos_swing, stikp[i].eefm_swing_pos_time_const); + stikp[i].target_ee_diff_p_filter->reset(stikp[i].d_pos_swing); + stikp[i].target_ee_diff_r_filter->reset(stikp[i].d_rpy_swing); + } else { + /* position */ + { + hrp::Vector3 tmpdiffp = stikp[i].eefm_swing_pos_spring_gain.cwiseProduct(stikp[i].target_ee_diff_p_filter->passFilter(stikp[i].target_ee_diff_p)); + double lvlimit = -50 * 1e-3 * dt, uvlimit = 50 * 1e-3 * dt; // 50 [mm/s] + hrp::Vector3 limit_by_lvlimit = stikp[i].prev_d_pos_swing + lvlimit * hrp::Vector3::Ones(); + hrp::Vector3 limit_by_uvlimit = stikp[i].prev_d_pos_swing + uvlimit * hrp::Vector3::Ones(); + stikp[i].d_pos_swing = vlimit(vlimit(tmpdiffp, -1 * limit_pos, limit_pos), limit_by_lvlimit, limit_by_uvlimit); + } + /* rotation */ + { + hrp::Vector3 tmpdiffr = stikp[i].eefm_swing_rot_spring_gain.cwiseProduct(stikp[i].target_ee_diff_r_filter->passFilter(hrp::rpyFromRot(stikp[i].target_ee_diff_r))); + double lvlimit = deg2rad(-20.0*dt), uvlimit = deg2rad(20.0*dt); // 20 [deg/s] + hrp::Vector3 limit_by_lvlimit = stikp[i].prev_d_rpy_swing + lvlimit * hrp::Vector3::Ones(); + hrp::Vector3 limit_by_uvlimit = stikp[i].prev_d_rpy_swing + uvlimit * hrp::Vector3::Ones(); + stikp[i].d_rpy_swing = vlimit(vlimit(tmpdiffr, -1 * limit_rot, limit_rot), limit_by_lvlimit, limit_by_uvlimit); + } + } + stikp[i].prev_d_pos_swing = stikp[i].d_pos_swing; + stikp[i].prev_d_rpy_swing = stikp[i].d_rpy_swing; + } + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] Swing foot control" << std::endl; for (size_t i = 0; i < stikp.size(); i++) { - if (is_ik_enable[i]) { - jpe_v[i]->calcInverseKinematics2Loop(tmpp_list.at(i), tmpR_list.at(i), 1.0, 0.001, 0.01, &qrefv, transition_smooth_gain, - //stikp[i].localCOPPos; - stikp[i].localp, - stikp[i].localR); - } + std::cerr << "[" << m_profile.instance_name << "] " + << "d_rpy_swing (" << stikp[i].ee_name << ") = " << (stikp[i].d_rpy_swing / M_PI * 180.0).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[deg], " + << "d_pos_swing (" << stikp[i].ee_name << ") = " << (stikp[i].d_pos_swing * 1e3).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl; + } + } +}; + +void Stabilizer::limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R) +{ + double tmp_d_pos_z_root = 0.0, prev_d_pos_z_root = d_pos_z_root; + if (use_limb_stretch_avoidance) { + for (size_t i = 0; i < stikp.size(); i++) { + if (is_ik_enable[i]) { + // Check whether inside limb length limitation + hrp::Link* parent_link = m_robot->link(stikp[i].parent_name); + hrp::Vector3 targetp = (ee_p[i] - ee_R[i] * stikp[i].localR.transpose() * stikp[i].localp) - parent_link->p; // position from parent to target link (world frame) + double limb_length_limitation = stikp[i].max_limb_length - stikp[i].limb_length_margin; + double tmp = limb_length_limitation * limb_length_limitation - targetp(0) * targetp(0) - targetp(1) * targetp(1); + if (targetp.norm() > limb_length_limitation && tmp >= 0) { + tmp_d_pos_z_root = std::min(tmp_d_pos_z_root, targetp(2) + std::sqrt(tmp)); } } + } + // Change root link height depending on limb length + d_pos_z_root = tmp_d_pos_z_root == 0.0 ? calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const) : tmp_d_pos_z_root; + } else { + d_pos_z_root = calcDampingControl(d_pos_z_root, limb_stretch_avoidance_time_const); + } + d_pos_z_root = vlimit(d_pos_z_root, prev_d_pos_z_root + limb_stretch_avoidance_vlimit[0], prev_d_pos_z_root + limb_stretch_avoidance_vlimit[1]); + m_robot->rootLink()->p(2) += d_pos_z_root; } // Damping control functions @@ -1419,12 +1734,49 @@ double Stabilizer::calcDampingControl (const double tau_d, const double tau, con return (1/DD * (tau_d - tau) - 1/TT * prev_d) * dt + prev_d; }; +// Retrieving only +hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT) +{ + return (- prev_d.cwiseQuotient(TT)) * dt + prev_d; +}; + +// Retrieving only +double Stabilizer::calcDampingControl (const double prev_d, const double TT) +{ + return - 1/TT * prev_d * dt + prev_d; +}; + hrp::Vector3 Stabilizer::calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d, const hrp::Vector3& DD, const hrp::Vector3& TT) { return ((tau_d - tau).cwiseQuotient(DD) - prev_d.cwiseQuotient(TT)) * dt + prev_d; }; +void Stabilizer::calcDiffFootOriginExtMoment () +{ + // calc reference ext moment around foot origin pos + // static const double grav = 9.80665; /* [m/s^2] */ + double mg = total_mass * eefm_gravitational_acceleration; + hrp::Vector3 ref_ext_moment = hrp::Vector3(mg * ref_cog(1) - ref_total_foot_origin_moment(0), + -mg * ref_cog(0) - ref_total_foot_origin_moment(1), + 0); + // calc act ext moment around foot origin pos + hrp::Vector3 act_ext_moment = hrp::Vector3(mg * act_cog(1) - act_total_foot_origin_moment(0), + -mg * act_cog(0) - act_total_foot_origin_moment(1), + 0); + // Do not calculate actual value if in the air, because of invalid act_zmp. + if ( !on_ground ) act_ext_moment = ref_ext_moment; + // Calc diff + diff_foot_origin_ext_moment = ref_ext_moment - act_ext_moment; + if (DEBUGP) { + std::cerr << "[" << m_profile.instance_name << "] DiffStaticBalancePointOffset" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] " + << "ref_ext_moment = " << ref_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm], " + << "act_ext_moment = " << act_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm], " + << "diff ext_moment = " << diff_foot_origin_ext_moment.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[mm]" << std::endl; + } +}; + /* RTC::ReturnCode_t Stabilizer::onAborting(RTC::UniqueId ec_id) { @@ -1462,23 +1814,25 @@ RTC::ReturnCode_t Stabilizer::onRateChanged(RTC::UniqueId ec_id) void Stabilizer::sync_2_st () { - std::cerr << "[" << m_profile.instance_name << "] " << "Sync IDLE => ST" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] Sync IDLE => ST" << std::endl; pangx_ref = pangy_ref = pangx = pangy = 0; rdx = rdy = rx = ry = 0; d_rpy[0] = d_rpy[1] = 0; pdr = hrp::Vector3::Zero(); pos_ctrl = hrp::Vector3::Zero(); for (size_t i = 0; i < stikp.size(); i++) { - target_ee_diff_p[i] = hrp::Vector3::Zero(); - target_ee_diff_r[i] = hrp::Vector3::Zero(); - prev_target_ee_diff_r[i] = hrp::Vector3::Zero(); - d_rpy_swing[i] = hrp::Vector3::Zero(); - d_pos_swing[i] = hrp::Vector3::Zero(); STIKParam& ikp = stikp[i]; - ikp.d_foot_pos = ikp.d_foot_rpy = ikp.ee_d_foot_rpy = hrp::Vector3::Zero(); + ikp.target_ee_diff_p = hrp::Vector3::Zero(); + ikp.target_ee_diff_r = hrp::Matrix33::Identity(); + ikp.d_pos_swing = ikp.prev_d_pos_swing = hrp::Vector3::Zero(); + ikp.d_rpy_swing = ikp.prev_d_rpy_swing = hrp::Vector3::Zero(); + ikp.target_ee_diff_p_filter->reset(hrp::Vector3::Zero()); + ikp.target_ee_diff_r_filter->reset(hrp::Vector3::Zero()); + ikp.d_foot_pos = ikp.ee_d_foot_pos = ikp.d_foot_rpy = ikp.ee_d_foot_rpy = hrp::Vector3::Zero(); } if (on_ground) { - transition_count = -1 * transition_time / dt; + transition_count = -1 * calcMaxTransitionCount(); control_mode = MODE_ST; } else { transition_count = 0; @@ -1488,8 +1842,9 @@ void Stabilizer::sync_2_st () void Stabilizer::sync_2_idle () { - std::cerr << "[" << m_profile.instance_name << "] " << "Sync ST => IDLE" << std::endl; - transition_count = transition_time / dt; + std::cerr << "[" << m_profile.instance_name << "] [" << m_qRef.tm + << "] Sync ST => IDLE" << std::endl; + transition_count = calcMaxTransitionCount(); for (int i = 0; i < m_robot->numJoints(); i++ ) { transition_joint_q[i] = m_robot->joint(i)->q; } @@ -1497,23 +1852,30 @@ void Stabilizer::sync_2_idle () void Stabilizer::startStabilizer(void) { - if ( transition_count == 0 && control_mode == MODE_IDLE ) { - std::cerr << "[" << m_profile.instance_name << "] " << "Start ST" << std::endl; - sync_2_st(); + waitSTTransition(); // Wait until all transition has finished + { + Guard guard(m_mutex); + if ( control_mode == MODE_IDLE ) { + std::cerr << "[" << m_profile.instance_name << "] " << "Start ST" << std::endl; + sync_2_st(); + } + } waitSTTransition(); std::cerr << "[" << m_profile.instance_name << "] " << "Start ST DONE" << std::endl; - } } void Stabilizer::stopStabilizer(void) { - if ( transition_count == 0 && (control_mode == MODE_ST || control_mode == MODE_AIR) ) { - std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST" << std::endl; - control_mode = MODE_SYNC_TO_IDLE; - while (control_mode != MODE_IDLE) { usleep(10); }; + waitSTTransition(); // Wait until all transition has finished + { + Guard guard(m_mutex); + if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) { + std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST" << std::endl; + control_mode = (control_mode == MODE_ST) ? MODE_SYNC_TO_IDLE : MODE_IDLE; + } + } waitSTTransition(); std::cerr << "[" << m_profile.instance_name << "] " << "Stop ST DONE" << std::endl; - } } void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) @@ -1544,6 +1906,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_body_attitude_control_gain[i] = eefm_body_attitude_control_gain[i]; i_stp.ref_capture_point[i] = ref_cp(i); i_stp.act_capture_point[i] = act_cp(i); + i_stp.cp_offset[i] = cp_offset(i); } i_stp.eefm_pos_time_const_support.length(stikp.size()); i_stp.eefm_pos_damping_gain.length(stikp.size()); @@ -1556,6 +1919,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_swing_rot_spring_gain.length(stikp.size()); i_stp.eefm_swing_rot_time_const.length(stikp.size()); i_stp.eefm_ee_moment_limit.length(stikp.size()); + i_stp.eefm_ee_forcemoment_distribution_weight.length(stikp.size()); for (size_t j = 0; j < stikp.size(); j++) { i_stp.eefm_pos_damping_gain[j].length(3); i_stp.eefm_pos_time_const_support[j].length(3); @@ -1566,6 +1930,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_swing_rot_spring_gain[j].length(3); i_stp.eefm_swing_rot_time_const[j].length(3); i_stp.eefm_ee_moment_limit[j].length(3); + i_stp.eefm_ee_forcemoment_distribution_weight[j].length(6); for (size_t i = 0; i < 3; i++) { i_stp.eefm_pos_damping_gain[j][i] = stikp[j].eefm_pos_damping_gain(i); i_stp.eefm_pos_time_const_support[j][i] = stikp[j].eefm_pos_time_const_support(i); @@ -1576,10 +1941,16 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_swing_rot_spring_gain[j][i] = stikp[j].eefm_swing_rot_spring_gain(i); i_stp.eefm_swing_rot_time_const[j][i] = stikp[j].eefm_swing_rot_time_const(i); i_stp.eefm_ee_moment_limit[j][i] = stikp[j].eefm_ee_moment_limit(i); + i_stp.eefm_ee_forcemoment_distribution_weight[j][i] = stikp[j].eefm_ee_forcemoment_distribution_weight(i); + i_stp.eefm_ee_forcemoment_distribution_weight[j][i+3] = stikp[j].eefm_ee_forcemoment_distribution_weight(i+3); } i_stp.eefm_pos_compensation_limit[j] = stikp[j].eefm_pos_compensation_limit; i_stp.eefm_rot_compensation_limit[j] = stikp[j].eefm_rot_compensation_limit; } + for (size_t i = 0; i < 3; i++) { + i_stp.eefm_swing_pos_damping_gain[i] = eefm_swing_pos_damping_gain(i); + i_stp.eefm_swing_rot_damping_gain[i] = eefm_swing_rot_damping_gain(i); + } i_stp.eefm_pos_time_const_swing = eefm_pos_time_const_swing; i_stp.eefm_pos_transition_time = eefm_pos_transition_time; i_stp.eefm_pos_margin_time = eefm_pos_margin_time; @@ -1603,11 +1974,13 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_wrench_alpha_blending = szd->get_wrench_alpha_blending(); i_stp.eefm_alpha_cutoff_freq = szd->get_alpha_cutoff_freq(); i_stp.eefm_gravitational_acceleration = eefm_gravitational_acceleration; - i_stp.eefm_ee_pos_error_p_gain = eefm_ee_pos_error_p_gain; - i_stp.eefm_ee_rot_error_p_gain = eefm_ee_rot_error_p_gain; - i_stp.eefm_ee_error_cutoff_freq = target_ee_diff_p_filter[0]->getCutOffFreq(); + i_stp.eefm_ee_error_cutoff_freq = stikp[0].target_ee_diff_p_filter->getCutOffFreq(); i_stp.eefm_use_force_difference_control = eefm_use_force_difference_control; - + i_stp.eefm_use_swing_damping = eefm_use_swing_damping; + for (size_t i = 0; i < 3; ++i) { + i_stp.eefm_swing_damping_force_thre[i] = eefm_swing_damping_force_thre[i]; + i_stp.eefm_swing_damping_moment_thre[i] = eefm_swing_damping_moment_thre[i]; + } i_stp.is_ik_enable.length(is_ik_enable.size()); for (size_t i = 0; i < is_ik_enable.size(); i++) { i_stp.is_ik_enable[i] = is_ik_enable[i]; @@ -1634,6 +2007,9 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) for (size_t i = 0; i < cp_check_margin.size(); i++) { i_stp.cp_check_margin[i] = cp_check_margin[i]; } + for (size_t i = 0; i < tilt_margin.size(); i++) { + i_stp.tilt_margin[i] = tilt_margin[i]; + } i_stp.contact_decision_threshold = contact_decision_threshold; i_stp.is_estop_while_walking = is_estop_while_walking; switch(control_mode) { @@ -1646,6 +2022,15 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) } i_stp.emergency_check_mode = emergency_check_mode; i_stp.end_effector_list.length(stikp.size()); + i_stp.use_limb_stretch_avoidance = use_limb_stretch_avoidance; + i_stp.use_zmp_truncation = use_zmp_truncation; + i_stp.limb_stretch_avoidance_time_const = limb_stretch_avoidance_time_const; + i_stp.limb_length_margin.length(stikp.size()); + i_stp.detection_time_to_air = detection_count_to_air * dt; + for (size_t i = 0; i < 2; i++) { + i_stp.limb_stretch_avoidance_vlimit[i] = limb_stretch_avoidance_vlimit[i]; + i_stp.root_rot_compensation_limit[i] = root_rot_compensation_limit[i]; + } for (size_t i = 0; i < stikp.size(); i++) { const rats::coordinates cur_ee = rats::coordinates(stikp.at(i).localp, stikp.at(i).localR); OpenHRP::AutoBalancerService::Footstep ret_ee; @@ -1661,6 +2046,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) ret_ee.leg = stikp.at(i).ee_name.c_str(); // set i_stp.end_effector_list[i] = ret_ee; + i_stp.limb_length_margin[i] = stikp[i].limb_length_margin; } i_stp.ik_limb_parameters.length(jpe_v.size()); for (size_t i = 0; i < jpe_v.size(); i++) { @@ -1676,6 +2062,7 @@ void Stabilizer::getParameter(OpenHRP::StabilizerService::stParam& i_stp) ilp.avoid_gain = stikp[i].avoid_gain; ilp.reference_gain = stikp[i].reference_gain; ilp.manipulability_limit = jpe_v[i]->getManipulabilityLimit(); + ilp.ik_loop_count = stikp[i].ik_loop_count; // size_t -> unsigned short, value may change, but ik_loop_count is small value and value not change } }; @@ -1690,10 +2077,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) k_brot_tc[i] = i_stp.k_brot_tc[i]; } std::cerr << "[" << m_profile.instance_name << "] TPCC" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] k_tpcc_p = [" << k_tpcc_p[0] << ", " << k_tpcc_p[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] k_tpcc_x = [" << k_tpcc_x[0] << ", " << k_tpcc_x[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] k_brot_p = [" << k_brot_p[0] << ", " << k_brot_p[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] k_brot_tc = [" << k_brot_tc[0] << ", " << k_brot_tc[1] << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] k_tpcc_p = [" << k_tpcc_p[0] << ", " << k_tpcc_p[1] << "], k_tpcc_x = [" << k_tpcc_x[0] << ", " << k_tpcc_x[1] << "], k_brot_p = [" << k_brot_p[0] << ", " << k_brot_p[1] << "], k_brot_tc = [" << k_brot_tc[0] << ", " << k_brot_tc[1] << "]" << std::endl; // for (size_t i = 0; i < 2; i++) { // k_run_b[i] = i_stp.k_run_b[i]; // d_run_b[i] = i_stp.d_run_b[i]; @@ -1721,6 +2105,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) eefm_body_attitude_control_time_const[i] = i_stp.eefm_body_attitude_control_time_const[i]; ref_cp(i) = i_stp.ref_capture_point[i]; act_cp(i) = i_stp.act_capture_point[i]; + cp_offset(i) = i_stp.cp_offset[i]; } bool is_damping_parameter_ok = true; if ( i_stp.eefm_pos_damping_gain.length () == stikp.size() && @@ -1733,7 +2118,8 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) i_stp.eefm_rot_compensation_limit.length () == stikp.size() && i_stp.eefm_swing_rot_spring_gain.length () == stikp.size() && i_stp.eefm_swing_rot_time_const.length () == stikp.size() && - i_stp.eefm_ee_moment_limit.length () == stikp.size() ) { + i_stp.eefm_ee_moment_limit.length () == stikp.size() && + i_stp.eefm_ee_forcemoment_distribution_weight.length () == stikp.size()) { is_damping_parameter_ok = true; for (size_t j = 0; j < stikp.size(); j++) { for (size_t i = 0; i < 3; i++) { @@ -1746,6 +2132,8 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) stikp[j].eefm_swing_rot_spring_gain(i) = i_stp.eefm_swing_rot_spring_gain[j][i]; stikp[j].eefm_swing_rot_time_const(i) = i_stp.eefm_swing_rot_time_const[j][i]; stikp[j].eefm_ee_moment_limit(i) = i_stp.eefm_ee_moment_limit[j][i]; + stikp[j].eefm_ee_forcemoment_distribution_weight(i) = i_stp.eefm_ee_forcemoment_distribution_weight[j][i]; + stikp[j].eefm_ee_forcemoment_distribution_weight(i+3) = i_stp.eefm_ee_forcemoment_distribution_weight[j][i+3]; } stikp[j].eefm_pos_compensation_limit = i_stp.eefm_pos_compensation_limit[j]; stikp[j].eefm_rot_compensation_limit = i_stp.eefm_rot_compensation_limit[j]; @@ -1753,6 +2141,10 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) } else { is_damping_parameter_ok = false; } + for (size_t i = 0; i < 3; i++) { + eefm_swing_pos_damping_gain(i) = i_stp.eefm_swing_pos_damping_gain[i]; + eefm_swing_rot_damping_gain(i) = i_stp.eefm_swing_rot_damping_gain[i]; + } eefm_pos_time_const_swing = i_stp.eefm_pos_time_const_swing; eefm_pos_transition_time = i_stp.eefm_pos_transition_time; eefm_pos_margin_time = i_stp.eefm_pos_margin_time; @@ -1779,18 +2171,22 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) szd->print_vertices(std::string(m_profile.instance_name)); } eefm_use_force_difference_control = i_stp.eefm_use_force_difference_control; - + eefm_use_swing_damping = i_stp.eefm_use_swing_damping; + for (size_t i = 0; i < 3; ++i) { + eefm_swing_damping_force_thre[i] = i_stp.eefm_swing_damping_force_thre[i]; + eefm_swing_damping_moment_thre[i] = i_stp.eefm_swing_damping_moment_thre[i]; + } act_cogvel_filter->setCutOffFreq(i_stp.eefm_cogvel_cutoff_freq); szd->set_wrench_alpha_blending(i_stp.eefm_wrench_alpha_blending); szd->set_alpha_cutoff_freq(i_stp.eefm_alpha_cutoff_freq); eefm_gravitational_acceleration = i_stp.eefm_gravitational_acceleration; - eefm_ee_pos_error_p_gain = i_stp.eefm_ee_pos_error_p_gain; - eefm_ee_rot_error_p_gain = i_stp.eefm_ee_rot_error_p_gain; - for (size_t i = 0; i < target_ee_diff_p_filter.size(); i++) { - target_ee_diff_p_filter[i]->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq); + for (size_t i = 0; i < stikp.size(); i++) { + stikp[i].target_ee_diff_p_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq); + stikp[i].target_ee_diff_r_filter->setCutOffFreq(i_stp.eefm_ee_error_cutoff_freq); + stikp[i].limb_length_margin = i_stp.limb_length_margin[i]; } setBoolSequenceParam(is_ik_enable, i_stp.is_ik_enable, std::string("is_ik_enable")); - setBoolSequenceParam(is_feedback_control_enable, i_stp.is_feedback_control_enable, std::string("is_feedback_control_enable")); + setBoolSequenceParamWithCheckContact(is_feedback_control_enable, i_stp.is_feedback_control_enable, std::string("is_feedback_control_enable")); setBoolSequenceParam(is_zmp_calc_enable, i_stp.is_zmp_calc_enable, std::string("is_zmp_calc_enable")); emergency_check_mode = i_stp.emergency_check_mode; @@ -1799,8 +2195,20 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) for (size_t i = 0; i < cp_check_margin.size(); i++) { cp_check_margin[i] = i_stp.cp_check_margin[i]; } + szd->set_vertices_from_margin_params(cp_check_margin); + for (size_t i = 0; i < tilt_margin.size(); i++) { + tilt_margin[i] = i_stp.tilt_margin[i]; + } contact_decision_threshold = i_stp.contact_decision_threshold; is_estop_while_walking = i_stp.is_estop_while_walking; + use_limb_stretch_avoidance = i_stp.use_limb_stretch_avoidance; + use_zmp_truncation = i_stp.use_zmp_truncation; + limb_stretch_avoidance_time_const = i_stp.limb_stretch_avoidance_time_const; + for (size_t i = 0; i < 2; i++) { + limb_stretch_avoidance_vlimit[i] = i_stp.limb_stretch_avoidance_vlimit[i]; + root_rot_compensation_limit[i] = i_stp.root_rot_compensation_limit[i]; + } + detection_count_to_air = static_cast(i_stp.detection_time_to_air / dt); if (control_mode == MODE_IDLE) { for (size_t i = 0; i < i_stp.end_effector_list.length(); i++) { std::vector::iterator it = std::find_if(stikp.begin(), stikp.end(), (&boost::lambda::_1->* &std::vector::value_type::ee_name == std::string(i_stp.end_effector_list[i].leg))); @@ -1811,9 +2219,9 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) std::cerr << "[" << m_profile.instance_name << "] cannot change end-effectors except during MODE_IDLE" << std::endl; } for (std::vector::const_iterator it = stikp.begin(); it != stikp.end(); it++) { - std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->ee_name << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] End Effector [" << it->ee_name << "]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] localpos = " << it->localp.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] localR = " << it->localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "\n", " [", "]")) << std::endl; + std::cerr << "[" << m_profile.instance_name << "] localR = " << it->localR.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", "", " [", "]")) << std::endl; } if (i_stp.foot_origin_offset.length () != 2) { std::cerr << "[" << m_profile.instance_name << "] foot_origin_offset cannot be set. Length " << i_stp.foot_origin_offset.length() << " != " << 2 << std::endl; @@ -1831,13 +2239,9 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) std::cerr << foot_origin_offset[i].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")); } std::cerr << "[m]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_k1 = [" << eefm_k1[0] << ", " << eefm_k1[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_k2 = [" << eefm_k2[0] << ", " << eefm_k2[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_k3 = [" << eefm_k3[0] << ", " << eefm_k3[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_zmp_delay_time_const = [" << eefm_zmp_delay_time_const[0] << ", " << eefm_zmp_delay_time_const[1] << "][s]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_ref_zmp_aux = [" << ref_zmp_aux(0) << ", " << ref_zmp_aux(1) << "][m]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_body_attitude_control_gain = [" << eefm_body_attitude_control_gain[0] << ", " << eefm_body_attitude_control_gain[1] << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_body_attitude_control_time_const = [" << eefm_body_attitude_control_time_const[0] << ", " << eefm_body_attitude_control_time_const[1] << "][s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_k1 = [" << eefm_k1[0] << ", " << eefm_k1[1] << "], eefm_k2 = [" << eefm_k2[0] << ", " << eefm_k2[1] << "], eefm_k3 = [" << eefm_k3[0] << ", " << eefm_k3[1] << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_zmp_delay_time_const = [" << eefm_zmp_delay_time_const[0] << ", " << eefm_zmp_delay_time_const[1] << "][s], eefm_ref_zmp_aux = [" << ref_zmp_aux(0) << ", " << ref_zmp_aux(1) << "][m]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_body_attitude_control_gain = [" << eefm_body_attitude_control_gain[0] << ", " << eefm_body_attitude_control_gain[1] << "], eefm_body_attitude_control_time_const = [" << eefm_body_attitude_control_time_const[0] << ", " << eefm_body_attitude_control_time_const[1] << "][s]" << std::endl; if (is_damping_parameter_ok) { for (size_t j = 0; j < stikp.size(); j++) { std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] eefm_rot_damping_gain = " @@ -1851,8 +2255,9 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) << stikp[j].eefm_pos_time_const_support.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[s]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] " - << "eefm_pos_compensation_limit = " << stikp[j].eefm_pos_compensation_limit << "[m], " << " " - << "eefm_rot_compensation_limit = " << stikp[j].eefm_rot_compensation_limit << "[rad]" << std::endl; + << "eefm_pos_compensation_limit = " << stikp[j].eefm_pos_compensation_limit << "[m], " + << "eefm_rot_compensation_limit = " << stikp[j].eefm_rot_compensation_limit << "[rad], " + << "eefm_ee_moment_limit = " << stikp[j].eefm_ee_moment_limit.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] " << "eefm_swing_pos_spring_gain = " << stikp[j].eefm_swing_pos_spring_gain.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " << "eefm_swing_pos_time_const = " << stikp[j].eefm_swing_pos_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " @@ -1860,7 +2265,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) << "eefm_swing_pos_time_const = " << stikp[j].eefm_swing_pos_time_const.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << ", " << std::endl; std::cerr << "[" << m_profile.instance_name << "] [" << stikp[j].ee_name << "] " - << "eefm_ee_moment_limit = " << stikp[j].eefm_ee_moment_limit.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[Nm]" << std::endl; + << "eefm_ee_forcemoment_distribution_weight = " << stikp[j].eefm_ee_forcemoment_distribution_weight.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "" << std::endl; } } else { std::cerr << "[" << m_profile.instance_name << "] eefm damping parameters cannot be set because of invalid param." << std::endl; @@ -1868,8 +2273,8 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) std::cerr << "[" << m_profile.instance_name << "] eefm_pos_transition_time = " << eefm_pos_transition_time << "[s], eefm_pos_margin_time = " << eefm_pos_margin_time << "[s] eefm_pos_time_const_swing = " << eefm_pos_time_const_swing << "[s]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] cogvel_cutoff_freq = " << act_cogvel_filter->getCutOffFreq() << "[Hz]" << std::endl; szd->print_params(std::string(m_profile.instance_name)); - std::cerr << "[" << m_profile.instance_name << "] eefm_gravitational_acceleration = " << eefm_gravitational_acceleration << "[m/s^2], eefm_use_force_difference_control = " << (eefm_use_force_difference_control? "true":"false") << std::endl; - std::cerr << "[" << m_profile.instance_name << "] eefm_ee_pos_error_p_gain = " << eefm_ee_pos_error_p_gain << ", eefm_ee_rot_error_p_gain = " << eefm_ee_rot_error_p_gain << ", eefm_ee_error_cutoff_freq = " << target_ee_diff_p_filter[0]->getCutOffFreq() << "[Hz]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_gravitational_acceleration = " << eefm_gravitational_acceleration << "[m/s^2], eefm_use_force_difference_control = " << (eefm_use_force_difference_control? "true":"false") << ", eefm_use_swing_damping = " << (eefm_use_swing_damping? "true":"false") << std::endl; + std::cerr << "[" << m_profile.instance_name << "] eefm_ee_error_cutoff_freq = " << stikp[0].target_ee_diff_p_filter->getCutOffFreq() << "[Hz]" << std::endl; std::cerr << "[" << m_profile.instance_name << "] COMMON" << std::endl; if (control_mode == MODE_IDLE) { st_algorithm = i_stp.st_algorithm; @@ -1878,10 +2283,12 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) std::cerr << "[" << m_profile.instance_name << "] st_algorithm cannot be changed to [" << getStabilizerAlgorithmString(st_algorithm) << "] during MODE_AIR or MODE_ST." << std::endl; } std::cerr << "[" << m_profile.instance_name << "] emergency_check_mode changed to [" << (emergency_check_mode == OpenHRP::StabilizerService::NO_CHECK?"NO_CHECK": (emergency_check_mode == OpenHRP::StabilizerService::COP?"COP":"CP") ) << "]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] transition_time = " << transition_time << "[s]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] cop_check_margin = " << cop_check_margin << "[m]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] cp_check_margin = [" << cp_check_margin[0] << ", " << cp_check_margin[1] << ", " << cp_check_margin[2] << ", " << cp_check_margin[3] << "] [m]" << std::endl; - std::cerr << "[" << m_profile.instance_name << "] contact_decision_threshold = " << contact_decision_threshold << "[N]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] transition_time = " << transition_time << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] cop_check_margin = " << cop_check_margin << "[m], " + << "cp_check_margin = [" << cp_check_margin[0] << ", " << cp_check_margin[1] << ", " << cp_check_margin[2] << ", " << cp_check_margin[3] << "] [m], " + << "tilt_margin = [" << tilt_margin[0] << ", " << tilt_margin[1] << "] [rad]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] contact_decision_threshold = " << contact_decision_threshold << "[N], detection_time_to_air = " << detection_count_to_air * dt << "[s]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] root_rot_compensation_limit = [" << root_rot_compensation_limit[0] << " " << root_rot_compensation_limit[1] << "][rad]" << std::endl; // IK limb parameters std::cerr << "[" << m_profile.instance_name << "] IK limb parameters" << std::endl; bool is_ik_limb_parameter_valid_length = true; @@ -1906,6 +2313,7 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) stikp[i].avoid_gain = ilp.avoid_gain; stikp[i].reference_gain = ilp.reference_gain; jpe_v[i]->setManipulabilityLimit(ilp.manipulability_limit); + stikp[i].ik_loop_count = ilp.ik_loop_count; // unsigned short -> size_t, value not change } } else { std::cerr << "[" << m_profile.instance_name << "] ik_optional_weight_vector invalid length! Cannot be set. (input = ["; @@ -1952,6 +2360,11 @@ void Stabilizer::setParameter(const OpenHRP::StabilizerService::stParam& i_stp) std::cerr << jpe_v[i]->getManipulabilityLimit() << ", "; } std::cerr << "]" << std::endl; + std::cerr << "[" << m_profile.instance_name << "] ik_loop_count = ["; + for (size_t i = 0; i < stikp.size(); i++) { + std::cerr << stikp[i].ik_loop_count << ", "; + } + std::cerr << "]" << std::endl; } } @@ -1966,6 +2379,8 @@ std::string Stabilizer::getStabilizerAlgorithmString (OpenHRP::StabilizerService return "EEFMQP"; case OpenHRP::StabilizerService::EEFMQPCOP: return "EEFMQPCOP"; + case OpenHRP::StabilizerService::EEFMQPCOP2: + return "EEFMQPCOP2"; default: return ""; } @@ -1973,9 +2388,12 @@ std::string Stabilizer::getStabilizerAlgorithmString (OpenHRP::StabilizerService void Stabilizer::setBoolSequenceParam (std::vector& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name) { + std::vector prev_values; + prev_values.resize(st_bool_values.size()); + copy (st_bool_values.begin(), st_bool_values.end(), prev_values.begin()); if (st_bool_values.size() != output_bool_values.length()) { std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set. Length " << st_bool_values.size() << " != " << output_bool_values.length() << std::endl; - } else if (control_mode != MODE_IDLE) { + } else if ( (control_mode != MODE_IDLE) ) { std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set. Current control_mode is " << control_mode << std::endl; } else { for (size_t i = 0; i < st_bool_values.size(); i++) { @@ -1986,12 +2404,73 @@ void Stabilizer::setBoolSequenceParam (std::vector& st_bool_values, const for (size_t i = 0; i < st_bool_values.size(); i++) { std::cerr <<"[" << st_bool_values[i] << "]"; } - std::cerr << std::endl; + std::cerr << "(set = "; + for (size_t i = 0; i < output_bool_values.length(); i++) { + std::cerr <<"[" << output_bool_values[i] << "]"; + } + std::cerr << ", prev = "; + for (size_t i = 0; i < prev_values.size(); i++) { + std::cerr <<"[" << prev_values[i] << "]"; + } + std::cerr << ")" << std::endl; +}; + +void Stabilizer::setBoolSequenceParamWithCheckContact (std::vector& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name) +{ + std::vector prev_values; + prev_values.resize(st_bool_values.size()); + copy (st_bool_values.begin(), st_bool_values.end(), prev_values.begin()); + if (st_bool_values.size() != output_bool_values.length()) { + std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set. Length " << st_bool_values.size() << " != " << output_bool_values.length() << std::endl; + } else if ( control_mode == MODE_IDLE ) { + for (size_t i = 0; i < st_bool_values.size(); i++) { + st_bool_values[i] = output_bool_values[i]; + } + } else { + std::vector failed_indices; + for (size_t i = 0; i < st_bool_values.size(); i++) { + if ( (st_bool_values[i] != output_bool_values[i]) ) { // If mode change + if (!ref_contact_states[i] ) { // reference contact_states should be OFF + st_bool_values[i] = output_bool_values[i]; + } else { + failed_indices.push_back(i); + } + } + } + if (failed_indices.size() > 0) { + std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " cannot be set partially. failed_indices is ["; + for (size_t i = 0; i < failed_indices.size(); i++) { + std::cerr << failed_indices[i] << " "; + } + std::cerr << "]" << std::endl; + } + } + std::cerr << "[" << m_profile.instance_name << "] " << prop_name << " is "; + for (size_t i = 0; i < st_bool_values.size(); i++) { + std::cerr <<"[" << st_bool_values[i] << "]"; + } + std::cerr << "(set = "; + for (size_t i = 0; i < output_bool_values.length(); i++) { + std::cerr <<"[" << output_bool_values[i] << "]"; + } + std::cerr << ", prev = "; + for (size_t i = 0; i < prev_values.size(); i++) { + std::cerr <<"[" << prev_values[i] << "]"; + } + std::cerr << ")" << std::endl; }; void Stabilizer::waitSTTransition() { - while (transition_count != 0) usleep(10); + // Wait condition + // 1. Check transition_count : Wait until transition is finished + // 2. Check control_mode : Once control_mode is SYNC mode, wait until control_mode moves to the next mode (MODE_AIR or MODE_IDLE) + bool flag = (control_mode == MODE_SYNC_TO_AIR || control_mode == MODE_SYNC_TO_IDLE); + while (transition_count != 0 || + (flag ? !(control_mode == MODE_IDLE || control_mode == MODE_AIR) : false) ) { + usleep(10); + flag = (control_mode == MODE_SYNC_TO_AIR || control_mode == MODE_SYNC_TO_IDLE); + } usleep(10); } @@ -2035,6 +2514,21 @@ hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, const hrp::Vector3& l return ret; } +hrp::Vector3 Stabilizer::vlimit(const hrp::Vector3& value, const hrp::Vector3& llimit_value, const hrp::Vector3& ulimit_value) +{ + hrp::Vector3 ret; + for (size_t i = 0; i < 3; i++) { + if (value(i) > ulimit_value(i)) { + ret(i) = ulimit_value(i); + } else if (value(i) < llimit_value(i)) { + ret(i) = llimit_value(i); + } else { + ret(i) = value(i); + } + } + return ret; +} + static double switching_inpact_absorber(double force, double lower_th, double upper_th) { double gradient, intercept; diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index 1cad8f7b095..e6e59d86328 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -10,6 +10,8 @@ #ifndef STABILIZER_COMPONENT_H #define STABILIZER_COMPONENT_H +#include +#include #include #include #include @@ -116,9 +118,12 @@ class Stabilizer void calcSwingSupportLimbGain(); void calcTPCC(); void calcEEForceMomentControl(); + void calcSwingEEModification (); + void limbStretchAvoidanceControl (const std::vector& ee_p, const std::vector& ee_R); void getParameter(OpenHRP::StabilizerService::stParam& i_stp); void setParameter(const OpenHRP::StabilizerService::stParam& i_stp); void setBoolSequenceParam (std::vector& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name); + void setBoolSequenceParamWithCheckContact (std::vector& st_bool_values, const OpenHRP::StabilizerService::BoolSequence& output_bool_values, const std::string& prop_name); std::string getStabilizerAlgorithmString (OpenHRP::StabilizerService::STAlgorithm _st_algorithm); void waitSTTransition(); // funcitons for calc final torque output @@ -128,16 +133,24 @@ class Stabilizer void getFootmidCoords (rats::coordinates& ret); double calcDampingControl (const double tau_d, const double tau, const double prev_d, const double DD, const double TT); + hrp::Vector3 calcDampingControl (const hrp::Vector3& prev_d, const hrp::Vector3& TT); + double calcDampingControl (const double prev_d, const double TT); hrp::Vector3 calcDampingControl (const hrp::Vector3& tau_d, const hrp::Vector3& tau, const hrp::Vector3& prev_d, const hrp::Vector3& DD, const hrp::Vector3& TT); double vlimit(double value, double llimit_value, double ulimit_value); hrp::Vector3 vlimit(const hrp::Vector3& value, double llimit_value, double ulimit_value); hrp::Vector3 vlimit(const hrp::Vector3& value, const hrp::Vector3& limit_value); + hrp::Vector3 vlimit(const hrp::Vector3& value, const hrp::Vector3& llimit_value, const hrp::Vector3& ulimit_value); inline bool isContact (const size_t idx) // 0 = right, 1 = left { return (prev_act_force_z[idx] > 25.0); }; + inline int calcMaxTransitionCount () + { + return (transition_time / dt); + }; + void calcDiffFootOriginExtMoment (); protected: // Configuration variable declaration @@ -152,9 +165,12 @@ class Stabilizer RTC::TimedPoint3D m_zmp; RTC::TimedPoint3D m_refCP; RTC::TimedPoint3D m_actCP; + RTC::TimedPoint3D m_diffCP; + RTC::TimedPoint3D m_diffFootOriginExtMoment; RTC::TimedPoint3D m_basePos; RTC::TimedOrientation3D m_baseRpy; RTC::TimedBooleanSeq m_contactStates; + RTC::TimedDoubleSeq m_toeheelRatio; RTC::TimedDoubleSeq m_controlSwingSupportTime; std::vector m_limbCOPOffset; RTC::TimedBooleanSeq m_actContactStates; @@ -182,6 +198,7 @@ class Stabilizer RTC::InPort m_basePosIn; RTC::InPort m_baseRpyIn; RTC::InPort m_contactStatesIn; + RTC::InPort m_toeheelRatioIn; RTC::InPort m_controlSwingSupportTimeIn; std::vector *> m_limbCOPOffsetIn; RTC::InPort m_qRefSeqIn; @@ -202,6 +219,8 @@ class Stabilizer RTC::OutPort m_zmpOut; RTC::OutPort m_refCPOut; RTC::OutPort m_actCPOut; + RTC::OutPort m_diffCPOut; + RTC::OutPort m_diffFootOriginExtMomentOut; RTC::OutPort m_actContactStatesOut; RTC::OutPort m_COPInfoOut; RTC::OutPort m_emergencySignalOut; @@ -240,20 +259,28 @@ class Stabilizer std::string target_name; // Name of end link std::string ee_name; // Name of ee (e.g., rleg, lleg, ...) std::string sensor_name; // Name of force sensor in the limb + std::string parent_name; // Name of parent ling in the limb hrp::Vector3 localp; // Position of ee in end link frame (^{l}p_e = R_l^T (p_e - p_l)) hrp::Vector3 localCOPPos; // Position offset of reference COP in end link frame (^{l}p_{cop} = R_l^T (p_{cop} - p_l) - ^{l}p_e) hrp::Matrix33 localR; // Rotation of ee in end link frame (^{l}R_e = R_l^T R_e) // For eefm - hrp::Vector3 d_foot_pos, d_foot_rpy, ee_d_foot_rpy; + hrp::Vector3 d_foot_pos, ee_d_foot_pos, d_foot_rpy, ee_d_foot_rpy; hrp::Vector3 eefm_pos_damping_gain, eefm_pos_time_const_support, eefm_rot_damping_gain, eefm_rot_time_const, eefm_swing_rot_spring_gain, eefm_swing_pos_spring_gain, eefm_swing_rot_time_const, eefm_swing_pos_time_const, eefm_ee_moment_limit; double eefm_pos_compensation_limit, eefm_rot_compensation_limit; hrp::Vector3 ref_force, ref_moment; + hrp::dvector6 eefm_ee_forcemoment_distribution_weight; double swing_support_gain, support_time; + // For swing ee modification + boost::shared_ptr > target_ee_diff_p_filter, target_ee_diff_r_filter; + hrp::Vector3 target_ee_diff_p, d_pos_swing, d_rpy_swing, prev_d_pos_swing, prev_d_rpy_swing; + hrp::Matrix33 target_ee_diff_r; // IK parameter - double avoid_gain, reference_gain; + double avoid_gain, reference_gain, max_limb_length, limb_length_margin; + size_t ik_loop_count; }; enum cmode {MODE_IDLE, MODE_AIR, MODE_ST, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_AIR} control_mode; // members + std::map m_vfs; std::vector jpe_v; hrp::BodyPtr m_robot; coil::Mutex m_mutex; @@ -261,26 +288,30 @@ class Stabilizer hrp::dvector transition_joint_q, qorg, qrefv; std::vector stikp; std::map contact_states_index_map; - std::vector contact_states, prev_contact_states, is_ik_enable, is_feedback_control_enable, is_zmp_calc_enable; + std::vector ref_contact_states, prev_ref_contact_states, act_contact_states, is_ik_enable, is_feedback_control_enable, is_zmp_calc_enable; + std::vector toeheel_ratio; double dt; int transition_count, loop; - bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, initial_cp_too_large_error; + int m_is_falling_counter; + std::vector m_will_fall_counter; + int is_air_counter, detection_count_to_air; + bool is_legged_robot, on_ground, is_emergency, is_seq_interpolating, reset_emergency_flag, eefm_use_force_difference_control, eefm_use_swing_damping, initial_cp_too_large_error, use_limb_stretch_avoidance, use_zmp_truncation; bool is_walking, is_estop_while_walking; hrp::Vector3 current_root_p, target_root_p; - hrp::Matrix33 current_root_R, target_root_R, prev_act_foot_origin_rot, prev_ref_foot_origin_rot, target_foot_origin_rot; - std::vector target_ee_p, target_ee_diff_p, target_ee_diff_r, prev_target_ee_diff_r, rel_ee_pos, d_pos_swing, d_rpy_swing; + hrp::Matrix33 current_root_R, target_root_R, prev_act_foot_origin_rot, prev_ref_foot_origin_rot, target_foot_origin_rot, ref_foot_origin_rot; + std::vector target_ee_p, rel_ee_pos, act_ee_p, projected_normal, act_force, ref_force, ref_moment; std::vector target_ee_R, rel_ee_rot, act_ee_R; std::vector rel_ee_name; rats::coordinates target_foot_midcoords; hrp::Vector3 ref_zmp, ref_cog, ref_cp, ref_cogvel, rel_ref_cp, prev_ref_cog, prev_ref_zmp; - hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset; + hrp::Vector3 act_zmp, act_cog, act_cogvel, act_cp, rel_act_zmp, rel_act_cp, prev_act_cog, act_base_rpy, current_base_rpy, current_base_pos, sbp_cog_offset, cp_offset, diff_cp; hrp::Vector3 foot_origin_offset[2]; std::vector prev_act_force_z; - double zmp_origin_off, transition_smooth_gain; + double zmp_origin_off, transition_smooth_gain, d_pos_z_root, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], root_rot_compensation_limit[2]; boost::shared_ptr > act_cogvel_filter; - std::vector > > target_ee_diff_p_filter; OpenHRP::StabilizerService::STAlgorithm st_algorithm; SimpleZMPDistributor* szd; + std::vector > support_polygon_vetices, margined_support_polygon_vetices; // TPCC double k_tpcc_p[2], k_tpcc_x[2], d_rpy[2], k_brot_p[2], k_brot_tc[2]; // RUN ST @@ -292,11 +323,16 @@ class Stabilizer double rdx, rdy, rx, ry; // EEFM ST double eefm_k1[2], eefm_k2[2], eefm_k3[2], eefm_zmp_delay_time_const[2], eefm_body_attitude_control_gain[2], eefm_body_attitude_control_time_const[2]; - double eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_gravitational_acceleration, eefm_ee_pos_error_p_gain, eefm_ee_rot_error_p_gain; - hrp::Vector3 new_refzmp, rel_cog, ref_zmp_aux; + double eefm_pos_time_const_swing, eefm_pos_transition_time, eefm_pos_margin_time, eefm_gravitational_acceleration; + std::vector eefm_swing_damping_force_thre, eefm_swing_damping_moment_thre; + hrp::Vector3 new_refzmp, rel_cog, ref_zmp_aux, diff_foot_origin_ext_moment; hrp::Vector3 pos_ctrl; + hrp::Vector3 ref_total_force, ref_total_moment; + // Total foot moment around the foot origin coords (relative to foot origin coords) + hrp::Vector3 ref_total_foot_origin_moment, act_total_foot_origin_moment; + hrp::Vector3 eefm_swing_pos_damping_gain, eefm_swing_rot_damping_gain; double total_mass, transition_time, cop_check_margin, contact_decision_threshold; - std::vector cp_check_margin; + std::vector cp_check_margin, tilt_margin; OpenHRP::StabilizerService::EmergencyCheckMode emergency_check_mode; }; diff --git a/rtc/Stabilizer/StabilizerService_impl.h b/rtc/Stabilizer/StabilizerService_impl.h index 598a305a608..fa240d059b8 100644 --- a/rtc/Stabilizer/StabilizerService_impl.h +++ b/rtc/Stabilizer/StabilizerService_impl.h @@ -2,7 +2,7 @@ #ifndef __KALMANFILTER_SERVICE_H__ #define __KALMANFILTER_SERVICE_H__ -#include "StabilizerService.hh" +#include "hrpsys/idl/StabilizerService.hh" class Stabilizer; diff --git a/rtc/Stabilizer/ZMPDistributor.h b/rtc/Stabilizer/ZMPDistributor.h index 7b9f90440ee..96aea02a6fc 100644 --- a/rtc/Stabilizer/ZMPDistributor.h +++ b/rtc/Stabilizer/ZMPDistributor.h @@ -39,13 +39,15 @@ class FootSupportPolygon void get_vertices (std::vector >& vs) { vs = foot_vertices; }; void print_vertices (const std::string& str) { + std::cerr << "[" << str << "] "; for (size_t i = 0; i < foot_vertices.size(); i++) { - std::cerr << "[" << str << "] vs = "; + std::cerr << "vs = "; for (size_t j = 0; j < foot_vertices[i].size(); j++) { std::cerr << "[" << foot_vertices[i][j](0) << " " << foot_vertices[i][j](1) << "] "; } - std::cerr << "[m]" << std::endl;; + std::cerr << ((i==foot_vertices.size()-1)?"[m]":"[m], "); } + std::cerr << std::endl;; } }; @@ -53,9 +55,11 @@ class FootSupportPolygon class SimpleZMPDistributor { - FootSupportPolygon fs; + FootSupportPolygon fs, fs_mgn; double leg_inside_margin, leg_outside_margin, leg_front_margin, leg_rear_margin, wrench_alpha_blending; boost::shared_ptr > alpha_filter; + std::vector convex_hull; + enum projected_point_region {LEFT, MIDDLE, RIGHT}; public: enum leg_type {RLEG, LLEG, RARM, LARM, BOTH, ALL}; SimpleZMPDistributor (const double _dt) : wrench_alpha_blending (0.5) @@ -76,59 +80,34 @@ class SimpleZMPDistributor { return leg_pos(0) <= (-1 * leg_rear_margin + margin); }; - inline bool is_cp_inside_foot (const hrp::Vector3& cp, const leg_type support_leg, const double margin = 0.0, const double offset = 0.0) + inline bool is_inside_support_polygon (Eigen::Vector2d& p, const hrp::Vector3& offset = hrp::Vector3::Zero(), const bool& truncate_p = false, const std::string& str = "") { - if (support_leg == RLEG) return (cp(1) <= (leg_inside_margin - margin)) && (cp(1) >= (-1 * leg_outside_margin + margin)) && (cp(0) <= (leg_front_margin - margin)) && (cp(0) >= (-1 * leg_rear_margin + margin)); - else if (support_leg == LLEG) return (cp(1) >= (-1 * leg_inside_margin + margin)) && (cp(1) <= (leg_outside_margin - margin)) && (cp(0) <= (leg_front_margin - margin)) && (cp(0) >= (-1 * leg_rear_margin + margin)); - else if (support_leg == BOTH) return (cp(1) <= (leg_outside_margin + offset - margin)) && (cp(1) >= (-1 * (leg_outside_margin + offset) + margin)) && (cp(0) <= (leg_front_margin - margin)) && (cp(0) >= (-1 * leg_rear_margin + margin)); - else return true; - }; - inline bool is_inside_support_polygon (Eigen::Vector2d& p, const std::vector& ee_pos, const std::vector & ee_rot, const std::vector& ee_name, const leg_type& support_leg, const std::vector& tmp_margin = std::vector(), const hrp::Vector3& offset = hrp::Vector3(0.0, 0.0, 0.0)) - { - size_t l_idx, r_idx; - for (size_t i = 0; i < ee_name.size(); i++) { - if (ee_name[i]=="rleg") r_idx = i; - else if (ee_name[i]=="lleg") l_idx = i; - } - std::vector rleg_vertices; - std::vector lleg_vertices; - std::vector convex_vertices; - - // assume that each foot vertices has four vertices - std::vector margin(4, 0.0); - for (size_t i = 0; i < tmp_margin.size(); i++) { - margin[i] = tmp_margin[i]; - } - // RLEG - rleg_vertices.push_back(Eigen::Vector2d(ee_pos[r_idx](0) + leg_front_margin - margin[0] + offset(0), ee_pos[r_idx](1) + leg_inside_margin - margin[2] + offset(1))); - rleg_vertices.push_back(Eigen::Vector2d(ee_pos[r_idx](0) + leg_front_margin - margin[0] + offset(0), ee_pos[r_idx](1) + -1*(leg_outside_margin - margin[3]) + offset(1))); - rleg_vertices.push_back(Eigen::Vector2d(ee_pos[r_idx](0) + -1*(leg_rear_margin - margin[1]) + offset(0), ee_pos[r_idx](1) + -1*(leg_outside_margin - margin[3]) + offset(1))); - rleg_vertices.push_back(Eigen::Vector2d(ee_pos[r_idx](0) + -1*(leg_rear_margin - margin[1]) + offset(0), ee_pos[r_idx](1) + leg_inside_margin - margin[2] + offset(1))); - // LLEG - lleg_vertices.push_back(Eigen::Vector2d(ee_pos[l_idx](0) + leg_front_margin - margin[0] + offset(0), ee_pos[l_idx](1) + leg_outside_margin - margin[3] + offset(1))); - lleg_vertices.push_back(Eigen::Vector2d(ee_pos[l_idx](0) + leg_front_margin - margin[0] + offset(0), ee_pos[l_idx](1) + -1*(leg_inside_margin - margin[2]) + offset(1))); - lleg_vertices.push_back(Eigen::Vector2d(ee_pos[l_idx](0) + -1*(leg_rear_margin - margin[1]) + offset(0), ee_pos[l_idx](1) + -1*(leg_inside_margin - margin[2]) + offset(1))); - lleg_vertices.push_back(Eigen::Vector2d(ee_pos[l_idx](0) + -1*(leg_rear_margin - margin[1]) + offset(0), ee_pos[l_idx](1) + leg_outside_margin - margin[3] + offset(1))); - - if (support_leg == BOTH) { - // sort vertices in clockwise order - convex_vertices.push_back(lleg_vertices[0]); - convex_vertices.push_back(lleg_vertices[1]); - std::copy(rleg_vertices.begin(),rleg_vertices.end(),std::back_inserter(convex_vertices)); - convex_vertices.push_back(lleg_vertices[2]); - convex_vertices.push_back(lleg_vertices[3]); - convex_vertices = calcConvexHull(convex_vertices); - } else if (support_leg == RLEG) { - convex_vertices = rleg_vertices; - } else if (support_leg == LLEG) { - convex_vertices = lleg_vertices; + // set any inner point(ip) and binary search two vertices(convex_hull[v_a], convex_hull[v_b]) between which p is. + p -= offset.head(2); + size_t n_ch = convex_hull.size(); + Eigen::Vector2d ip = (convex_hull[0] + convex_hull[n_ch/3] + convex_hull[2*n_ch/3]) / 3.0; + size_t v_a = 0, v_b = n_ch; + while (v_a + 1 < v_b) { + size_t v_c = (v_a + v_b) / 2; + if (calcCrossProduct(convex_hull[v_a], convex_hull[v_c], ip) > 0) { + if (calcCrossProduct(convex_hull[v_a], p, ip) > 0 && calcCrossProduct(convex_hull[v_c], p, ip) < 0) v_b = v_c; + else v_a = v_c; + } else { + if (calcCrossProduct(convex_hull[v_a], p, ip) < 0 && calcCrossProduct(convex_hull[v_c], p, ip) > 0) v_a = v_c; + else v_b = v_c; + } } - // check whether p is inside support polygon - for (size_t i = 0; i < convex_vertices.size() - 1; i++) { - if (calcCrossProduct(p, convex_vertices[i + 1], convex_vertices[i]) < 0) return false; + v_b %= n_ch; + if (calcCrossProduct(convex_hull[v_a], convex_hull[v_b], p) >= 0) { + p += offset.head(2); + return true; + } else { + if (truncate_p) { + if (!calc_closest_boundary_point(p, v_a, v_b)) std::cerr << "[" << str << "] Cannot calculate closest boundary point on the convex hull" << std::endl; + } + p += offset.head(2); + return false; } - if (calcCrossProduct(p, convex_vertices.front(), convex_vertices.back()) < 0) return false; - return true; }; void print_params (const std::string& str) { @@ -139,6 +118,64 @@ class SimpleZMPDistributor { fs.print_vertices(str); }; + // Compare Vector2d for sorting lexicographically + static bool compare_eigen2d(const Eigen::Vector2d& lv, const Eigen::Vector2d& rv) + { + return lv(0) < rv(0) || (lv(0) == rv(0) && lv(1) < rv(1)); + }; + // Calculate 2D convex hull based on Andrew's algorithm + // Assume that the order of vs, ee, and cs is the same + void calc_convex_hull (const std::vector >& vs, const std::vector& cs, const std::vector& ee_pos, const std::vector & ee_rot) + { + // transform coordinate + std::vector tvs; + hrp::Vector3 tpos; + tvs.reserve(cs.size()*vs[0].size()); + for (size_t i = 0; i < cs.size(); i++) { + if (cs[i]) { + for (size_t j = 0; j < vs[i].size(); j++) { + tpos = ee_pos[i] + ee_rot[i] * hrp::Vector3(vs[i][j](0), vs[i][j](1), 0.0); + tvs.push_back(tpos.head(2)); + } + } + } + // calculate convex hull + int n_tvs = tvs.size(), n_ch = 0; + convex_hull.resize(2*n_tvs); + std::sort(tvs.begin(), tvs.end(), compare_eigen2d); + for (int i = 0; i < n_tvs; convex_hull[n_ch++] = tvs[i++]) + while (n_ch >= 2 && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--; + for (int i = n_tvs-2, j = n_ch+1; i >= 0; convex_hull[n_ch++] = tvs[i--]) + while (n_ch >= j && calcCrossProduct(convex_hull[n_ch-1], tvs[i], convex_hull[n_ch-2]) <= 0) n_ch--; + convex_hull.resize(n_ch-1); + }; + // Calculate closest boundary point on the convex hull + bool calc_closest_boundary_point (Eigen::Vector2d& p, size_t& right_idx, size_t& left_idx) { + size_t n_ch = convex_hull.size(); + Eigen::Vector2d cur_closest_point; + for (size_t i = 0; i < n_ch; i++) { + switch(calcProjectedPoint(cur_closest_point, p, convex_hull[left_idx], convex_hull[right_idx])) { + case MIDDLE: + p = cur_closest_point; + return true; + case LEFT: + right_idx = left_idx; + left_idx = (left_idx + 1) % n_ch; + if ((p - convex_hull[right_idx]).dot(convex_hull[left_idx] - convex_hull[right_idx]) <= 0) { + p = cur_closest_point; + return true; + } + case RIGHT: + left_idx = right_idx; + right_idx = (right_idx - 1) % n_ch; + if ((p - convex_hull[left_idx]).dot(convex_hull[right_idx] - convex_hull[left_idx]) <= 0) { + p = cur_closest_point; + return true; + } + } + } + return false; + } // setter void set_wrench_alpha_blending (const double a) { wrench_alpha_blending = a; }; void set_leg_front_margin (const double a) { leg_front_margin = a; }; @@ -192,6 +229,30 @@ class SimpleZMPDistributor // } set_vertices(vec); }; + // Set vertices only for cp_check_margin for now + void set_vertices_from_margin_params (const std::vector& margin) + { + std::vector > vec; + // RLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[2])); + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[3]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[3]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[2])); + vec.push_back(tvec); + } + // LLEG + { + std::vector tvec; + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], leg_inside_margin - margin[3])); + tvec.push_back(Eigen::Vector2d(leg_front_margin - margin[0], -1*(leg_outside_margin - margin[2]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), -1*(leg_outside_margin - margin[2]))); + tvec.push_back(Eigen::Vector2d(-1*(leg_rear_margin - margin[1]), leg_inside_margin - margin[3])); + vec.push_back(tvec); + } + fs_mgn.set_vertices(vec); + }; // getter double get_wrench_alpha_blending () { return wrench_alpha_blending; }; double get_leg_front_margin () { return leg_front_margin; }; @@ -200,6 +261,7 @@ class SimpleZMPDistributor double get_leg_outside_margin () { return leg_outside_margin; }; double get_alpha_cutoff_freq () { return alpha_filter->getCutOffFreq(); }; void get_vertices (std::vector >& vs) { fs.get_vertices(vs); }; + void get_margined_vertices (std::vector >& vs) { fs_mgn.get_vertices(vs); }; // double calcAlpha (const hrp::Vector3& tmprefzmp, const std::vector& ee_pos, @@ -350,6 +412,7 @@ class SimpleZMPDistributor const std::vector& ee_rot, const std::vector& ee_name, const std::vector& limb_gains, + const std::vector& toeheel_ratio, const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "") { @@ -563,6 +626,7 @@ class SimpleZMPDistributor const std::vector& ee_rot, const std::vector& ee_name, const std::vector& limb_gains, + const std::vector& toeheel_ratio, const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "", const bool use_cop_distribution = false) @@ -579,6 +643,7 @@ class SimpleZMPDistributor // QP double norm_weight = 1e-7; double cop_weight = 1e-3; + double ref_force_weight = 0;// 1e-3; hrp::dvector total_fm(3); total_fm(0) = total_fz; total_fm(1) = 0; @@ -593,6 +658,7 @@ class SimpleZMPDistributor double alpha_thre = 1e-20; // fz_alpha inversion for weighing matrix for (size_t i = 0; i < fz_alpha_vector.size(); i++) { + fz_alpha_vector[i] *= limb_gains[i]; fz_alpha_vector[i] = (fz_alpha_vector[i] < alpha_thre) ? 1/alpha_thre : 1/fz_alpha_vector[i]; } for (size_t j = 0; j < fz_alpha_vector.size(); j++) { @@ -630,7 +696,8 @@ class SimpleZMPDistributor for (size_t i = 0; i < state_dim_one; i++) { Kmat(j,i+j*state_dim_one) = 1.0; } - reff(j) = total_fz/2.0; + reff(j) = ref_foot_force[j](2);// total_fz/2.0; + KW(j,j) = ref_force_weight; } Hmat += Kmat.transpose() * KW * Kmat; gvec += -1 * Kmat.transpose() * KW * reff; @@ -678,12 +745,13 @@ class SimpleZMPDistributor const std::vector& ee_rot, const std::vector& ee_name, const std::vector& limb_gains, + const std::vector& toeheel_ratio, const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "", const bool use_cop_distribution = false) { distributeZMPToForceMoments(ref_foot_force, ref_foot_moment, - ee_pos, cop_pos, ee_rot, ee_name, limb_gains, + ee_pos, cop_pos, ee_rot, ee_name, limb_gains, toeheel_ratio, new_refzmp, ref_zmp, total_fz, dt, printp, print_str); }; @@ -708,9 +776,10 @@ class SimpleZMPDistributor const std::vector& ee_rot, const std::vector& ee_name, const std::vector& limb_gains, + const std::vector& toeheel_ratio, const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "", - const bool use_cop_distribution = true) + const bool use_cop_distribution = true, const std::vector is_contact_list = std::vector()) { size_t ee_num = ee_name.size(); std::vector alpha_vector(ee_num), fz_alpha_vector(ee_num); @@ -724,7 +793,9 @@ class SimpleZMPDistributor double norm_moment_weight = 1e2; size_t total_wrench_dim = 5; size_t state_dim = 6*ee_num; - { // Temp + + // Temporarily set ref_foot_force and ref_foot_moment based on reference values + { size_t total_wrench_dim = 5; //size_t total_wrench_dim = 3; hrp::dmatrix Wmat = hrp::dmatrix::Identity(state_dim/2, state_dim/2); @@ -782,12 +853,14 @@ class SimpleZMPDistributor if (printp) { for (size_t i = 0; i < ee_num; i++) { std::cerr << "[" << print_str << "] " - << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl; - std::cerr << "[" << print_str << "] " + << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; } } + // Calculate force/moment distribution matrix and vector + // We assume F = G f, calculate F and G. f is absolute here. + // 1. Calculate F (total_wrench) hrp::dvector total_wrench = hrp::dvector::Zero(total_wrench_dim); hrp::Vector3 ref_total_force = hrp::Vector3::Zero(); for (size_t fidx = 0; fidx < ee_num; fidx++) { @@ -803,8 +876,7 @@ class SimpleZMPDistributor } total_wrench(3) -= -(ref_zmp(2) - new_refzmp(2)) * ref_total_force(1) + (ref_zmp(1) - new_refzmp(1)) * ref_total_force(2); total_wrench(4) -= (ref_zmp(2) - new_refzmp(2)) * ref_total_force(0) - (ref_zmp(0) - new_refzmp(0)) * ref_total_force(2); - - hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim); + // 2. Calculate G (Gmat) hrp::dmatrix Gmat = hrp::dmatrix::Zero(total_wrench_dim, state_dim); for (size_t j = 0; j < ee_num; j++) { for (size_t k = 0; k < total_wrench_dim; k++) Gmat(k,6*j+k) = 1.0; @@ -820,10 +892,49 @@ class SimpleZMPDistributor } } } + // Calc rotation matrix to introduce local frame + // G = [tmpsubG_0, tmpsubG_1, ...] + // R = diag[tmpR_0, tmpR_1, ...] + // f = R f_l + // f : absolute, f_l : local + // G f = G R f_l + // G R = [tmpsubG_0 tmpR_0, tmpsubG_1 tmpR_1, ...] -> inserted to Gmat + hrp::dmatrix tmpsubG = hrp::dmatrix::Zero(total_wrench_dim, 6); + hrp::dmatrix tmpR = hrp::dmatrix::Zero(6,6); + for (size_t ei = 0; ei < ee_num; ei++) { + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < 6; j++) { + tmpsubG(i,j) = Gmat(i,6*ei+j); + } + } + for (size_t i = 0; i < 3; i++) { + for (size_t j = 0; j < 3; j++) { + tmpR(i,j) = tmpR(i+3,j+3) = ee_rot[ei](i,j); + } + } + tmpsubG = tmpsubG * tmpR; + for (size_t i = 0; i < total_wrench_dim; i++) { + for (size_t j = 0; j < 6; j++) { + Gmat(i,6*ei+j) = tmpsubG(i,j); + } + } + } + + // Calc weighting matrix + hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim); for (size_t j = 0; j < ee_num; j++) { for (size_t i = 0; i < 3; i++) { Wmat(i+j*6, i+j*6) = fz_alpha_vector[j] * limb_gains[j]; Wmat(i+j*6+3, i+j*6+3) = (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j]; + // Set local Y moment + // If toeheel_ratio is 0, toe and heel contact and local Y moment should be 0. + if (i == 1) { + Wmat(i+j*6+3, i+j*6+3) = toeheel_ratio[j] * Wmat(i+j*6+3, i+j*6+3); + } + // In actual swing phase, X/Y momoment should be 0. + if (!is_contact_list.empty()) { + if (!is_contact_list[j]) Wmat(i+j*6+3, i+j*6+3) = 0; + } } } if (printp) { @@ -842,40 +953,198 @@ class SimpleZMPDistributor // std::cerr << "Wmat" << std::endl; // std::cerr << Wmat << std::endl; + // Solve hrp::dvector ret(state_dim); calcWeightedLinearEquation(ret, Gmat, Wmat, total_wrench); + // Extract force and moment with converting local frame -> absolute frame (ret is local frame) for (size_t fidx = 0; fidx < ee_num; fidx++) { - ref_foot_force[fidx] += hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); - ref_foot_moment[fidx] += hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); + ref_foot_force[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); + ref_foot_moment[fidx] += ee_rot[fidx] * hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); + } + if (printp) { + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + } + }; + + void distributeZMPToForceMomentsPseudoInverse2 (std::vector& ref_foot_force, std::vector& ref_foot_moment, + const std::vector& ee_pos, + const std::vector& cop_pos, + const std::vector& ee_rot, + const std::vector& ee_name, + const std::vector& limb_gains, + const std::vector& toeheel_ratio, + const hrp::Vector3& new_refzmp, const hrp::Vector3& ref_zmp, + const hrp::Vector3& total_force, const hrp::Vector3& total_moment, + const std::vector& ee_forcemoment_distribution_weight, + const double total_fz, const double dt, const bool printp = true, const std::string& print_str = "") + { +#define FORCE_MOMENT_DIFF_CONTROL + + size_t ee_num = ee_name.size(); + std::vector alpha_vector(ee_num), fz_alpha_vector(ee_num); + calcAlphaVectorFromCOPDistance(alpha_vector, fz_alpha_vector, cop_pos, ee_name, new_refzmp, ref_zmp); + if (printp) { + std::cerr << "[" << print_str << "] force moment distribution (Pinv2) "; +#ifdef FORCE_MOMENT_DIFF_CONTROL + std::cerr << "(FORCE_MOMENT_DIFF_CONTROL)" << std::endl; +#else + std::cerr << "(NOT FORCE_MOMENT_DIFF_CONTROL)" << std::endl; +#endif + } + // ref_foot_force and ref_foot_moment should be set + size_t state_dim = 6*ee_num; + if (printp) { + std::cerr << "[" << print_str << "] fz_alpha_vector ["; + for (size_t j = 0; j < ee_num; j++) { + std::cerr << fz_alpha_vector[j] << " "; + } + std::cerr << std::endl; + } + + hrp::dvector total_wrench = hrp::dvector::Zero(6); +#ifndef FORCE_MOMENT_DIFF_CONTROL + total_wrench(0) = total_force(0); + total_wrench(1) = total_force(1); + total_wrench(2) = total_force(2); +#endif + total_wrench(3) = total_moment(0); + total_wrench(4) = total_moment(1); + total_wrench(5) = total_moment(2); +#ifdef FORCE_MOMENT_DIFF_CONTROL + for (size_t fidx = 0; fidx < ee_num; fidx++) { + double tmp_tau_x = -(cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](1) + (cop_pos[fidx](1)-new_refzmp(1)) * ref_foot_force[fidx](2); + total_wrench(3) -= tmp_tau_x; + double tmp_tau_y = (cop_pos[fidx](2)-new_refzmp(2)) * ref_foot_force[fidx](0) - (cop_pos[fidx](0)-new_refzmp(0)) * ref_foot_force[fidx](2); + total_wrench(4) -= tmp_tau_y; + } +#endif + + hrp::dmatrix Wmat = hrp::dmatrix::Zero(state_dim, state_dim); + hrp::dmatrix Gmat = hrp::dmatrix::Zero(6, state_dim); + for (size_t j = 0; j < ee_num; j++) { + for (size_t k = 0; k < 6; k++) Gmat(k,6*j+k) = 1.0; + } + for (size_t i = 0; i < 6; i++) { + for (size_t j = 0; j < ee_num; j++) { + if ( i == 3 ) { // Nx + Gmat(i,6*j+1) = -(cop_pos[j](2) - new_refzmp(2)); + Gmat(i,6*j+2) = (cop_pos[j](1) - new_refzmp(1)); + } else if ( i == 4 ) { // Ny + Gmat(i,6*j) = (cop_pos[j](2) - new_refzmp(2)); + Gmat(i,6*j+2) = -(cop_pos[j](0) - new_refzmp(0)); + } + } + } + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 3; i++) { + Wmat(i+j*6, i+j*6) = ee_forcemoment_distribution_weight[j][i] * fz_alpha_vector[j] * limb_gains[j]; + //double norm_moment_weight = 1e2; + //Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * (1.0/norm_moment_weight) * fz_alpha_vector[j] * limb_gains[j]; + Wmat(i+j*6+3, i+j*6+3) = ee_forcemoment_distribution_weight[j][i+3] * fz_alpha_vector[j] * limb_gains[j]; + } + } + if (printp) { + std::cerr << "[" << print_str << "] newWmat(diag) = ["; + for (size_t j = 0; j < ee_num; j++) { + for (size_t i = 0; i < 6; i++) { + std::cerr << Wmat(i+j*6, i+j*6) << " "; + } + } + std::cerr << std::endl; + // std::cerr << "[" << print_str << "] Gmat"; + // std::cerr << Gmat << std::endl; + // std::cerr << "Wmat" << std::endl; + // std::cerr << Wmat << std::endl; + } + + hrp::dvector ret(state_dim); + // Consider 6DOF total wrench (Fx, Fy, Fz, Mx, My, Mz) + hrp::dmatrix selection_matrix = hrp::dmatrix::Identity(6,6); + // Consdier 3DOF total wrench (Fz, Mx, My) +// hrp::dmatrix selection_matrix = hrp::dmatrix::Zero(3,6); +// selection_matrix(0,2) = 1.0; +// selection_matrix(1,3) = 1.0; +// selection_matrix(2,4) = 1.0; + { + hrp::dvector selected_total_wrench = hrp::dvector::Zero(selection_matrix.rows()); + hrp::dmatrix selected_Gmat = hrp::dmatrix::Zero(selection_matrix.rows(), Gmat.cols()); + selected_total_wrench = selection_matrix * total_wrench; + selected_Gmat = selection_matrix * Gmat; + calcWeightedLinearEquation(ret, selected_Gmat, Wmat, selected_total_wrench); } + if (printp) { + hrp::dvector tmpretv(total_wrench.size()); + tmpretv = Gmat * ret - total_wrench; + std::cerr << "[" << print_str << "] " + << "test_diff " << tmpretv.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl; + std::cerr << "[" << print_str << "] " + << "total_wrench " << total_wrench.format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N][Nm]" << std::endl; for (size_t i = 0; i < ee_num; i++) { std::cerr << "[" << print_str << "] " - << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N]" << std::endl; + << "ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; + } + for (size_t i = 0; i < ee_num; i++) { +#ifdef FORCE_MOMENT_DIFF_CONTROL std::cerr << "[" << print_str << "] " + << "dif_ref_force [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; +#else + std::cerr << "[" << print_str << "] " + << "dif_ref_force [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i), ret(6*i+1), ret(6*i+2))-ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " + << "dif_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(hrp::Vector3(ret(6*i+3), ret(6*i+4), ret(6*i+5))-ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; +#endif + } + } + for (size_t fidx = 0; fidx < ee_num; fidx++) { +#ifdef FORCE_MOMENT_DIFF_CONTROL + ref_foot_force[fidx] += hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); + ref_foot_moment[fidx] += hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); +#else + ref_foot_force[fidx] = hrp::Vector3(ret(6*fidx), ret(6*fidx+1), ret(6*fidx+2)); + ref_foot_moment[fidx] = hrp::Vector3(ret(6*fidx+3), ret(6*fidx+4), ret(6*fidx+5)); +#endif + } + if (printp){ + for (size_t i = 0; i < ee_num; i++) { + std::cerr << "[" << print_str << "] " + << "new_ref_force [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_force[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[N], " << "new_ref_moment [" << ee_name[i] << "] " << hrp::Vector3(ref_foot_moment[i]).format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", "[", "]")) << "[Nm]" << std::endl; } } }; - double calcCrossProduct(Eigen::Vector2d& a, Eigen::Vector2d& b, Eigen::Vector2d& o) + double calcCrossProduct(const Eigen::Vector2d& a, const Eigen::Vector2d& b, const Eigen::Vector2d& o) { return (a(0) - o(0)) * (b(1) - o(1)) - (a(1) - o(1)) * (b(0) - o(0)); }; - // assume that vertices are listed in clockwise order - std::vector calcConvexHull(std::vector vertices) - { - std::vector convex_vertices; - - convex_vertices.push_back(vertices.front()); - for (size_t i = 1; i < vertices.size() - 1; i++) { - if (calcCrossProduct(vertices[i + 1], vertices[i - 1], vertices[i]) < 0) convex_vertices.push_back(vertices[i]); + projected_point_region calcProjectedPoint(Eigen::Vector2d& ret, const Eigen::Vector2d& target, const Eigen::Vector2d& a, const Eigen::Vector2d& b){ + Eigen::Vector2d v1 = target - b; + Eigen::Vector2d v2 = a - b; + double v2_norm = v2.norm(); + if ( v2_norm == 0 ) { + ret = a; + return LEFT; + } else { + double ratio = v1.dot(v2) / (v2_norm * v2_norm); + if (ratio < 0){ + ret = b; + return RIGHT; + } else if (ratio > 1){ + ret = a; + return LEFT; + } else { + ret = b + ratio * v2; + return MIDDLE; + } } - convex_vertices.push_back(vertices.back()); - - return convex_vertices; }; }; diff --git a/rtc/Stabilizer/testZMPDistributor.cpp b/rtc/Stabilizer/testZMPDistributor.cpp index 24eb93c215a..98d24023e0d 100644 --- a/rtc/Stabilizer/testZMPDistributor.cpp +++ b/rtc/Stabilizer/testZMPDistributor.cpp @@ -5,7 +5,7 @@ #include #include #include -#include "util/Hrpsys.h" // added for QNX compile +#include "hrpsys/util/Hrpsys.h" // added for QNX compile class testZMPDistributor { @@ -14,7 +14,7 @@ class testZMPDistributor double total_fz; // [N] SimpleZMPDistributor* szd; bool use_gnuplot; - enum {EEFM, EEFMQP, EEFMQP2} distribution_algorithm; + enum {EEFM, EEFMQP, EEFMQP2, EEFMQPCOP, EEFMQPCOP2} distribution_algorithm; size_t sleep_msec; std::vector leg_pos; std::vector ee_pos; @@ -25,7 +25,14 @@ class testZMPDistributor void gen_and_plot () { parse_params(); - std::cerr << "distribution_algorithm = " << ((distribution_algorithm==EEFM)?"EEFM": ((distribution_algorithm==EEFMQP)?"EEFMQP":"EEFMQP2") ) << ", sleep_msec = " << sleep_msec << "[msec]" << std::endl; + std::cerr << "distribution_algorithm = "; + if (distribution_algorithm==EEFM) std::cerr << "EEFM"; + else if (distribution_algorithm==EEFMQP) std::cerr << "EEFMQP"; + else if (distribution_algorithm==EEFMQP2) std::cerr << "EEFMQP(cop_distribution)"; + else if (distribution_algorithm==EEFMQPCOP) std::cerr << "EEFMQPCOP"; + else if (distribution_algorithm==EEFMQPCOP2) std::cerr << "EEFMQPCOP2"; + else std::cerr << "None?"; + std::cerr << ", sleep_msec = " << sleep_msec << "[msec]" << std::endl; szd->print_vertices(""); szd->print_params(std::string("")); // @@ -66,6 +73,7 @@ class testZMPDistributor names.push_back("rleg"); names.push_back("lleg"); std::vector limb_gains(names.size(), 1.0); + std::vector toeheel_ratio(names.size(), 1.0); std::vector ref_foot_force(names.size(), hrp::Vector3::Zero()), ref_foot_moment(names.size(), hrp::Vector3::Zero()); std::vector > fs; szd->get_vertices(fs); @@ -79,16 +87,27 @@ class testZMPDistributor // for (size_t i = 0; i < refzmp_vec.size(); i++) { double alpha = szd->calcAlpha(refzmp_vec[i], ee_pos, ee_rot, names); - if (distribution_algorithm == EEFMQP || distribution_algorithm == EEFMQP2) { - szd->distributeZMPToForceMomentsQP(ref_foot_force, ref_foot_moment, - ee_pos, cop_pos, ee_rot, names, limb_gains, - refzmp_vec[i], refzmp_vec[i], - total_fz, dt, true, "", (distribution_algorithm == EEFMQP2)); - } else { + if (distribution_algorithm == EEFMQP) { szd->distributeZMPToForceMoments(ref_foot_force, ref_foot_moment, - ee_pos, cop_pos, ee_rot, names, limb_gains, + ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio, refzmp_vec[i], refzmp_vec[i], total_fz, dt); + } else if (distribution_algorithm == EEFMQP || distribution_algorithm == EEFMQP2) { + szd->distributeZMPToForceMomentsQP(ref_foot_force, ref_foot_moment, + ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio, + refzmp_vec[i], refzmp_vec[i], + total_fz, dt, true, "", (distribution_algorithm == EEFMQP2)); + } else if (distribution_algorithm == EEFMQPCOP) { + szd->distributeZMPToForceMomentsPseudoInverse(ref_foot_force, ref_foot_moment, + ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio, + refzmp_vec[i], refzmp_vec[i], + total_fz, dt, true, ""); + } else if (distribution_algorithm == EEFMQPCOP2) { + // std::vector ee_forcemoment_distribution_weight(names.size(), 1.0); + // szd->distributeZMPToForceMomentsPseudoInverse2(ref_foot_force, ref_foot_moment, + // ee_pos, cop_pos, ee_rot, names, limb_gains, toeheel_ratio, + // refzmp_vec[i], refzmp_vec[i], + // total_fz, dt, true, ""); } for (size_t j = 0; j < fs.size(); j++) { std::string fname("/tmp/plot"+names[j]+".dat"); @@ -187,7 +206,23 @@ class testZMPDistributor { for (int i = 0; i < arg_strs.size(); ++ i) { if ( arg_strs[i]== "--distribution-algorithm" ) { - if (++i < arg_strs.size()) distribution_algorithm = ((arg_strs[i]=="EEFM")? EEFM : ((arg_strs[i]=="EEFMQP")? EEFMQP : EEFMQP2) ); + if (++i < arg_strs.size()) { + if (arg_strs[i]=="EEFM") { + distribution_algorithm = EEFM; + } else if (arg_strs[i]=="EEFMQP") { + distribution_algorithm = EEFMQP; + } else if (arg_strs[i]=="EEFMQP2") { + distribution_algorithm = EEFMQP2; + } else if (arg_strs[i]=="EEFMQPCOP") { + distribution_algorithm = EEFMQPCOP; + } else if (arg_strs[i]=="EEFMQPCOP") { + distribution_algorithm = EEFMQPCOP; + } else if (arg_strs[i]=="EEFMQPCOP2") { + distribution_algorithm = EEFMQPCOP2; + } else { + distribution_algorithm = EEFM; + } + } } else if ( arg_strs[i]== "--sleep-msec" ) { if (++i < arg_strs.size()) sleep_msec = atoi(arg_strs[i].c_str()); } else if ( arg_strs[i]== "--use-gnuplot" ) { diff --git a/rtc/StateHolder/CMakeLists.txt b/rtc/StateHolder/CMakeLists.txt index 64a8f415b30..192f00b7147 100644 --- a/rtc/StateHolder/CMakeLists.txt +++ b/rtc/StateHolder/CMakeLists.txt @@ -10,7 +10,7 @@ target_link_libraries(StateHolderComp ${libs}) set(target StateHolder StateHolderComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/StateHolder/StateHolder.cpp b/rtc/StateHolder/StateHolder.cpp index 2eb9e1553ea..5a681699440 100644 --- a/rtc/StateHolder/StateHolder.cpp +++ b/rtc/StateHolder/StateHolder.cpp @@ -54,8 +54,6 @@ StateHolder::StateHolder(RTC::Manager* manager) m_TimeKeeperServicePort("TimeKeeperService"), // m_timeCount(0), - m_waitSem(0), - m_timeSem(0), dummy(0) { @@ -63,6 +61,8 @@ StateHolder::StateHolder(RTC::Manager* manager) m_service1.setComponent(this); m_requestGoActual = false; + sem_init(&m_waitSem, 0, 0); + sem_init(&m_timeSem, 0, 0); } StateHolder::~StateHolder() @@ -150,9 +150,9 @@ RTC::ReturnCode_t StateHolder::onInitialize() // Setting for wrench data ports (real + virtual) std::vector fsensor_names; // find names for real force sensors - for ( int k = 0; k < lis->length(); k++ ) { + for ( unsigned int k = 0; k < lis->length(); k++ ) { OpenHRP::SensorInfoSequence& sensors = lis[k].sensors; - for ( int l = 0; l < sensors.length(); l++ ) { + for ( unsigned int l = 0; l < sensors.length(); l++ ) { if ( std::string(sensors[l].type) == "Force" ) { fsensor_names.push_back(std::string(sensors[l].name)); } @@ -161,12 +161,12 @@ RTC::ReturnCode_t StateHolder::onInitialize() int npforce = fsensor_names.size(); // find names for virtual force sensors coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ","); - int nvforce = virtual_force_sensor.size()/10; + unsigned int nvforce = virtual_force_sensor.size()/10; for (unsigned int i=0; i 0){ m_timeCount--; - if (m_timeCount == 0) m_timeSem.post(); + if (m_timeCount == 0) sem_post(&m_timeSem); } return RTC::RTC_OK; @@ -367,7 +367,7 @@ void StateHolder::goActual() { std::cout << "StateHolder::goActual()" << std::endl; m_requestGoActual = true; - m_waitSem.wait(); + sem_wait(&m_waitSem); } void StateHolder::getCommand(StateHolderService::Command &com) @@ -388,7 +388,7 @@ void StateHolder::getCommand(StateHolderService::Command &com) void StateHolder::wait(CORBA::Double tm) { m_timeCount = tm/m_dt; - m_timeSem.wait(); + sem_wait(&m_timeSem); } extern "C" diff --git a/rtc/StateHolder/StateHolder.h b/rtc/StateHolder/StateHolder.h index c9207acf047..d0b4ccca5b8 100644 --- a/rtc/StateHolder/StateHolder.h +++ b/rtc/StateHolder/StateHolder.h @@ -10,7 +10,9 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H -#include +#include +#include +#include #include #include #include @@ -165,7 +167,7 @@ class StateHolder private: int m_timeCount; - boost::interprocess::interprocess_semaphore m_waitSem, m_timeSem; + sem_t m_waitSem, m_timeSem; bool m_requestGoActual; double m_dt; int dummy; diff --git a/rtc/StateHolder/StateHolderService_impl.h b/rtc/StateHolder/StateHolderService_impl.h index 3e890f40ccd..94693a44657 100644 --- a/rtc/StateHolder/StateHolderService_impl.h +++ b/rtc/StateHolder/StateHolderService_impl.h @@ -2,7 +2,7 @@ #ifndef STATEHOLDERSERVICE_IMPL_H #define STATEHOLDERSERVICE_IMPL_H -#include "StateHolderService.hh" +#include "hrpsys/idl/StateHolderService.hh" using namespace OpenHRP; diff --git a/rtc/StateHolder/TimeKeeperService_impl.h b/rtc/StateHolder/TimeKeeperService_impl.h index 70ad5a4f51f..12fee1c6ffe 100644 --- a/rtc/StateHolder/TimeKeeperService_impl.h +++ b/rtc/StateHolder/TimeKeeperService_impl.h @@ -2,7 +2,7 @@ #ifndef TIMEKEEPERSERVICE_IMPL_H #define TIMEKEEPERSERVICE_IMPL_H -#include "TimeKeeperService.hh" +#include "hrpsys/idl/TimeKeeperService.hh" using namespace OpenHRP; diff --git a/rtc/ThermoEstimator/CMakeLists.txt b/rtc/ThermoEstimator/CMakeLists.txt index 7b0d1e7eebc..f02af1fd99f 100644 --- a/rtc/ThermoEstimator/CMakeLists.txt +++ b/rtc/ThermoEstimator/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(ThermoEstimatorComp ${libs}) set(target ThermoEstimator ThermoEstimatorComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ThermoEstimator/ThermoEstimator.cpp b/rtc/ThermoEstimator/ThermoEstimator.cpp index 7f39cc0dc6d..3df7e261990 100644 --- a/rtc/ThermoEstimator/ThermoEstimator.cpp +++ b/rtc/ThermoEstimator/ThermoEstimator.cpp @@ -13,7 +13,7 @@ */ #include "ThermoEstimator.h" -#include "RobotHardwareService.hh" +#include "hrpsys/idl/RobotHardwareService.hh" #include #include #include @@ -127,14 +127,14 @@ RTC::ReturnCode_t ThermoEstimator::onInitialize() std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motorHeatParams is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl; // motorHeatParam has default values itself } else { - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_motorHeatParams[i].temperature = m_ambientTemp; coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str()); coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str()); } if (m_debugLevel > 0) { std::cerr << "motorHeatParams is " << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << m_motorHeatParams[i].currentCoeffs << " " << m_motorHeatParams[i].thermoCoeffs << std::endl; } } @@ -147,12 +147,12 @@ RTC::ReturnCode_t ThermoEstimator::onInitialize() m_error2tau.resize(0); // invalid } else { m_error2tau.resize(m_robot->numJoints()); - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { coil::stringTo(m_error2tau[i], error2tauFromConf[i].c_str()); } if (m_debugLevel > 0) { std::cerr << "motorHeatParams:" << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << " " << m_error2tau[i]; } std::cerr << std::endl; @@ -200,7 +200,7 @@ RTC::ReturnCode_t ThermoEstimator::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id) { // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; - int numJoints = m_robot->numJoints(); + unsigned int numJoints = m_robot->numJoints(); m_loop++; coil::TimeValue coiltm(coil::gettimeofday()); @@ -225,12 +225,12 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id) hrp::dvector jointTorque; if (m_tauIn.data.length() == numJoints) { // use raw torque jointTorque.resize(numJoints); - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { jointTorque[i] = m_tauIn.data[i]; } if (isDebug()) { std::cerr << "raw torque: "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << m_tauIn.data[i] ; } std::cerr << std::endl; @@ -239,18 +239,18 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id) && m_qCurrentIn.data.length() == numJoints) { // estimate torque from joint error jointTorque.resize(numJoints); hrp::dvector jointError(numJoints); - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { jointError[i] = m_qRefIn.data[i] - m_qCurrentIn.data[i]; } estimateJointTorqueFromJointError(jointError, jointTorque); if (isDebug()) { std::cerr << "qRef: "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << m_qRefIn.data[i] ; } std::cerr << std::endl; std::cerr << "qCurrent: "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << m_qCurrentIn.data[i] ; } std::cerr << std::endl; @@ -261,7 +261,7 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id) // calculate temperature from joint torque if (jointTorque.size() == m_robot->numJoints()) { - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { // Thermo estimation calculateJointTemperature(jointTorque[i], m_motorHeatParams[i]); // output @@ -269,7 +269,7 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id) } if (isDebug()) { std::cerr << std::endl << "temperature : "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << m_motorHeatParams[i].temperature; } std::cerr << std::endl; @@ -292,7 +292,7 @@ RTC::ReturnCode_t ThermoEstimator::onExecute(RTC::UniqueId ec_id) } } else { // pass servoStateIn to servoStateOut m_servoStateOut.data.length(m_servoStateIn.data.length()); - for (int i = 0; i < m_servoStateIn.data.length(); i++) { + for (unsigned int i = 0; i < m_servoStateIn.data.length(); i++) { m_servoStateOut.data[i] = m_servoStateIn.data[i]; } } @@ -341,12 +341,12 @@ void ThermoEstimator::estimateJointTorqueFromJointError(hrp::dvector &error, hrp if (error.size() == m_robot->numJoints() && m_error2tau.size() == m_robot->numJoints()) { tau.resize(m_robot->numJoints()); - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { tau[i] = m_error2tau[i] * error[i]; } if (isDebug()) { std::cerr << "estimated torque: "; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << " " << tau[i] ; } std::cerr << std::endl; diff --git a/rtc/ThermoEstimator/ThermoEstimator.h b/rtc/ThermoEstimator/ThermoEstimator.h index 566fd004ee3..012c267fb00 100644 --- a/rtc/ThermoEstimator/ThermoEstimator.h +++ b/rtc/ThermoEstimator/ThermoEstimator.h @@ -10,6 +10,8 @@ #ifndef THERMO_ESTIMATOR_H #define THERMO_ESTIMATOR_H +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include @@ -21,7 +23,6 @@ #include #include -#include "HRPDataTypes.hh" #include "MotorHeatParam.h" // Service implementation headers diff --git a/rtc/ThermoLimiter/CMakeLists.txt b/rtc/ThermoLimiter/CMakeLists.txt index ff8e82a7ac9..7dcbfa71855 100644 --- a/rtc/ThermoLimiter/CMakeLists.txt +++ b/rtc/ThermoLimiter/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(ThermoLimiterComp ${libs}) set(target ThermoLimiter ThermoLimiterComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/ThermoLimiter/ThermoLimiter.cpp b/rtc/ThermoLimiter/ThermoLimiter.cpp index 11cd2ec74d1..1c34925622c 100644 --- a/rtc/ThermoLimiter/ThermoLimiter.cpp +++ b/rtc/ThermoLimiter/ThermoLimiter.cpp @@ -8,7 +8,6 @@ */ #include "ThermoLimiter.h" -#include "../SoftErrorLimiter/beep.h" #include #include #include @@ -43,10 +42,10 @@ ThermoLimiter::ThermoLimiter(RTC::Manager* manager) // m_tempInIn("tempIn", m_tempIn), m_tauMaxOutOut("tauMax", m_tauMaxOut), + m_beepCommandOutOut("beepCommand", m_beepCommandOut), m_ThermoLimiterServicePort("ThermoLimiterService"), m_debugLevel(0) { - init_beep(); m_ThermoLimiterService.thermolimiter(this); } @@ -70,6 +69,7 @@ RTC::ReturnCode_t ThermoLimiter::onInitialize() // Set OutPort buffer addOutPort("tauMax", m_tauMaxOutOut); + addOutPort("beepCommand", m_beepCommandOutOut); // Set service provider to Ports m_ThermoLimiterServicePort.registerProvider("service0", "ThermoLimiterService", m_ThermoLimiterService); @@ -105,11 +105,11 @@ RTC::ReturnCode_t ThermoLimiter::onInitialize() m_motorTemperatureLimit.resize(m_robot->numJoints()); if (motorTemperatureLimitFromConf.size() != m_robot->numJoints()) { std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motor_temperature_limit is " << motorTemperatureLimitFromConf.size() << ", not equal to " << m_robot->numJoints() << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_motorTemperatureLimit[i] = 80.0; } } else { - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { coil::stringTo(m_motorTemperatureLimit[i], motorTemperatureLimitFromConf[i].c_str()); } } @@ -133,13 +133,13 @@ RTC::ReturnCode_t ThermoLimiter::onInitialize() m_motorHeatParams.resize(m_robot->numJoints()); if (motorHeatParamsFromConf.size() != 2 * m_robot->numJoints()) { std::cerr << "[" << m_profile.instance_name << "] [WARN]: size of motor_heat_param is " << motorHeatParamsFromConf.size() << ", not equal to 2 * " << m_robot->numJoints() << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_motorHeatParams[i].defaultParams(); m_motorHeatParams[i].temperature = ambientTemp; } } else { - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_motorHeatParams[i].temperature = ambientTemp; coil::stringTo(m_motorHeatParams[i].currentCoeffs, motorHeatParamsFromConf[2 * i].c_str()); coil::stringTo(m_motorHeatParams[i].thermoCoeffs, motorHeatParamsFromConf[2 * i + 1].c_str()); @@ -153,7 +153,7 @@ RTC::ReturnCode_t ThermoLimiter::onInitialize() } std::cerr << std::endl; std::cerr << "default torque limit from model:" << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << m_robot->joint(i)->name << ":" << m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst << std::endl; } } @@ -170,6 +170,7 @@ RTC::ReturnCode_t ThermoLimiter::onInitialize() // allocate memory for outPorts m_tauMaxOut.data.length(m_robot->numJoints()); m_debug_print_freq = static_cast(0.1/m_dt); // once per 0.1 [s] + m_beepCommandOut.data.length(bc.get_num_beep_info()); return RTC::RTC_OK; } @@ -234,7 +235,7 @@ RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id) Guard guard(m_mutex); if (isDebug()) { std::cerr << "temperature: "; - for (int i = 0; i < m_tempIn.data.length(); i++) { + for (unsigned int i = 0; i < m_tempIn.data.length(); i++) { std::cerr << " " << m_tempIn.data[i]; } std::cerr << std::endl; @@ -244,7 +245,7 @@ RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id) if (m_tempIn.data.length() == m_robot->numJoints()) { calcMaxTorqueFromTemperature(tauMax); } else { - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { tauMax[i] = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst; // default torque limit from model } } @@ -266,11 +267,13 @@ RTC::ReturnCode_t ThermoLimiter::onExecute(RTC::UniqueId ec_id) callBeep(thermoLimitRatio, m_alarmRatio); // output restricted tauMax - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_tauMaxOut.data[i] = tauMax[i]; } m_tauMaxOut.tm = tm; m_tauMaxOutOut.write(); + m_beepCommandOut.tm = tm; + if (bc.isWritable()) m_beepCommandOutOut.write(); return RTC::RTC_OK; } @@ -311,13 +314,13 @@ RTC::ReturnCode_t ThermoLimiter::onRateChanged(RTC::UniqueId ec_id) void ThermoLimiter::calcMaxTorqueFromTemperature(hrp::dvector &tauMax) { - int numJoints = m_robot->numJoints(); + unsigned int numJoints = m_robot->numJoints(); double temp, tempLimit; hrp::dvector squareTauMax(numJoints); if (m_tempIn.data.length() == m_robot->numJoints()) { - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { temp = m_tempIn.data[i]; tempLimit = m_motorTemperatureLimit[i]; @@ -343,7 +346,7 @@ double ThermoLimiter::calcEmergencyRatio(RTC::TimedDoubleSeq ¤t, hrp::dvec { double maxEmergencyRatio = 0.0; if (current.data.length() == max.size()) { // estimate same dimension - for (int i = 0; i < current.data.length(); i++) { + for (unsigned int i = 0; i < current.data.length(); i++) { double tmpEmergencyRatio = std::abs(current.data[i] / max[i]); if (tmpEmergencyRatio > alarmRatio && m_loop % m_debug_print_freq == 0) { std::cerr << prefix << "[" << m_robot->joint(i)->name << "]" << " is over " << alarmRatio << " of the limit (" << current.data[i] << "/" << max[i] << ")" << std::endl; @@ -365,16 +368,17 @@ void ThermoLimiter::callBeep(double ratio, double alarmRatio) const int emergency_beep_cycle = 200; int current_emergency_beep_cycle = m_loop % emergency_beep_cycle; if (current_emergency_beep_cycle <= (emergency_beep_cycle / 2)) { - start_beep(4000, 60); + bc.startBeep(4000, 60); } else { - start_beep(2000, 60); + bc.startBeep(2000, 60); } } else if (ratio > alarmRatio) { // normal warning int freq = minFreq + (maxFreq - minFreq) * ((ratio - alarmRatio) / (1.0 - alarmRatio)); - start_beep(freq, 500); + bc.startBeep(freq, 500); } else { - stop_beep(); + bc.stopBeep(); } + bc.setDataPort(m_beepCommandOut); return; } diff --git a/rtc/ThermoLimiter/ThermoLimiter.h b/rtc/ThermoLimiter/ThermoLimiter.h index edb33152579..592d801ebc4 100644 --- a/rtc/ThermoLimiter/ThermoLimiter.h +++ b/rtc/ThermoLimiter/ThermoLimiter.h @@ -10,6 +10,7 @@ #ifndef THERMO_LIMITER_SERVICE_H #define THERMO_LIMITER_SERVICE_H +#include #include #include #include @@ -26,6 +27,7 @@ // Service implementation headers // #include "ThermoLimiterService_impl.h" +#include "../SoftErrorLimiter/beep.h" // @@ -111,6 +113,7 @@ class ThermoLimiter // TimedDoubleSeq m_tempIn; TimedDoubleSeq m_tauMaxOut; + TimedLongSeq m_beepCommandOut; // DataInPort declaration // @@ -121,6 +124,7 @@ class ThermoLimiter // DataOutPort declaration // OutPort m_tauMaxOutOut; + OutPort m_beepCommandOutOut; // @@ -150,6 +154,7 @@ class ThermoLimiter hrp::BodyPtr m_robot; std::vector m_motorHeatParams; coil::Mutex m_mutex; + BeepClient bc; void calcMaxTorqueFromTemperature(hrp::dvector &tauMax); double calcEmergencyRatio(RTC::TimedDoubleSeq ¤t, hrp::dvector &max, double alarmRatio, std::string &prefix); diff --git a/rtc/ThermoLimiter/ThermoLimiterService_impl.h b/rtc/ThermoLimiter/ThermoLimiterService_impl.h index 75a63abb529..41d96c10262 100644 --- a/rtc/ThermoLimiter/ThermoLimiterService_impl.h +++ b/rtc/ThermoLimiter/ThermoLimiterService_impl.h @@ -2,7 +2,7 @@ #ifndef THERMOLIMITERSERVICESVC_IMPL_H #define THERMOLIMITERSERVICESVC_IMPL_H -#include "ThermoLimiterService.hh" +#include "hrpsys/idl/ThermoLimiterService.hh" using namespace OpenHRP; diff --git a/rtc/TorqueController/CMakeLists.txt b/rtc/TorqueController/CMakeLists.txt index 856cc2e7412..494516137dc 100644 --- a/rtc/TorqueController/CMakeLists.txt +++ b/rtc/TorqueController/CMakeLists.txt @@ -14,6 +14,6 @@ target_link_libraries(testMotorTorqueController ${libs}) set(target TorqueController TorqueControllerComp testMotorTorqueController) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/TorqueController/MotorTorqueController.cpp b/rtc/TorqueController/MotorTorqueController.cpp index 90e4a1043c5..447d00917ec 100644 --- a/rtc/TorqueController/MotorTorqueController.cpp +++ b/rtc/TorqueController/MotorTorqueController.cpp @@ -9,7 +9,7 @@ */ #include "MotorTorqueController.h" -// #include "util/Hrpsys.h" +// #include "hrpsys/util/Hrpsys.h" #include #include #include diff --git a/rtc/TorqueController/TorqueController.cpp b/rtc/TorqueController/TorqueController.cpp index 5c31372c50f..2abdeec7742 100644 --- a/rtc/TorqueController/TorqueController.cpp +++ b/rtc/TorqueController/TorqueController.cpp @@ -8,7 +8,7 @@ */ #include "TorqueController.h" -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include #include @@ -43,8 +43,8 @@ TorqueController::TorqueController(RTC::Manager* manager) // m_tauCurrentInIn("tauCurrent", m_tauCurrentIn), m_tauMaxInIn("tauMax", m_tauMaxIn), - m_qRefInIn("qRef", m_qRefIn), m_qCurrentInIn("qCurrent", m_qCurrentIn), + m_qRefInIn("qRef", m_qRefIn), m_qRefOutOut("q", m_qRefOut), m_TorqueControllerServicePort("TorqueControllerService"), m_debugLevel(0) @@ -133,8 +133,8 @@ RTC::ReturnCode_t TorqueController::onInitialize() { // limit scope of tdc_dynamics_model_params std::cerr << "[" << m_profile.instance_name << "]" << "use TwoDofControllerDynamicsModel" << std::endl; std::vector tdc_dynamics_model_params(m_robot->numJoints()); - for (int i = 0; i < m_robot->numJoints(); i++) { - if (motorTorqueControllerParamsFromConf.size() == tdc_dynamics_model_params_num) { // use conf params if parameter num is correct + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { + if (motorTorqueControllerParamsFromConf.size() == (unsigned int)tdc_dynamics_model_params_num) { // use conf params if parameter num is correct coil::stringTo(tdc_dynamics_model_params[i].alpha, motorTorqueControllerParamsFromConf[4 * i].c_str()); coil::stringTo(tdc_dynamics_model_params[i].beta, motorTorqueControllerParamsFromConf[4 * i + 1].c_str()); coil::stringTo(tdc_dynamics_model_params[i].ki, motorTorqueControllerParamsFromConf[4 * i + 2].c_str()); @@ -145,7 +145,7 @@ RTC::ReturnCode_t TorqueController::onInitialize() } if (m_debugLevel > 0) { std::cerr << "[" << m_profile.instance_name << "]" << "torque controller parames:" << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << "[" << m_profile.instance_name << "]" << m_robot->joint(i)->name << ":" << tdc_dynamics_model_params[i].alpha << " " << tdc_dynamics_model_params[i].beta << " " << tdc_dynamics_model_params[i].ki << " " << tdc_dynamics_model_params[i].tc << " " << tdc_dynamics_model_params[i].dt << std::endl; @@ -157,8 +157,8 @@ RTC::ReturnCode_t TorqueController::onInitialize() { // limit scope of tdc_pd_model_params std::cerr << "[" << m_profile.instance_name << "]" << "use TwoDofControllerPDModel" << std::endl; std::vector tdc_pd_model_params(m_robot->numJoints()); - for (int i = 0; i < m_robot->numJoints(); i++) { - if (motorTorqueControllerParamsFromConf.size() == tdc_pd_model_params_num) { // use conf params if parameter num is correct + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { + if (motorTorqueControllerParamsFromConf.size() == (unsigned int)tdc_pd_model_params_num) { // use conf params if parameter num is correct coil::stringTo(tdc_pd_model_params[i].ke, motorTorqueControllerParamsFromConf[3 * i].c_str()); coil::stringTo(tdc_pd_model_params[i].kd, motorTorqueControllerParamsFromConf[3 * i + 1].c_str()); coil::stringTo(tdc_pd_model_params[i].tc, motorTorqueControllerParamsFromConf[3 * i + 2].c_str()); @@ -168,7 +168,7 @@ RTC::ReturnCode_t TorqueController::onInitialize() } if (m_debugLevel > 0) { std::cerr << "[" << m_profile.instance_name << "]" << "torque controller parames:" << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << "[" << m_profile.instance_name << "]" << m_robot->joint(i)->name << ":" << tdc_pd_model_params[i].ke << " " << tdc_pd_model_params[i].kd << " " << tdc_pd_model_params[i].tc << " " << tdc_pd_model_params[i].dt << std::endl; @@ -181,8 +181,8 @@ RTC::ReturnCode_t TorqueController::onInitialize() { // limit scope of tdc_params std::cerr << "[" << m_profile.instance_name << "]" << "use TwoDofController" << std::endl; std::vector tdc_params(m_robot->numJoints()); - for (int i = 0; i < m_robot->numJoints(); i++) { - if (motorTorqueControllerParamsFromConf.size() == tdc_params_num) { // use conf params if parameter num is correct + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { + if (motorTorqueControllerParamsFromConf.size() == (unsigned int)tdc_params_num) { // use conf params if parameter num is correct coil::stringTo(tdc_params[i].ke, motorTorqueControllerParamsFromConf[2 * i].c_str()); coil::stringTo(tdc_params[i].tc, motorTorqueControllerParamsFromConf[2 * i + 1].c_str()); } @@ -191,7 +191,7 @@ RTC::ReturnCode_t TorqueController::onInitialize() } if (m_debugLevel > 0) { std::cerr << "[" << m_profile.instance_name << "]" << "torque controller parames:" << std::endl; - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { std::cerr << "[" << m_profile.instance_name << "]" << m_robot->joint(i)->name << ":" << tdc_params[i].ke << " " << tdc_params[i].tc << " " << tdc_params[i].dt << std::endl; } @@ -209,7 +209,7 @@ RTC::ReturnCode_t TorqueController::onInitialize() std::cerr << "[" << m_profile.instance_name << "]" << "size of torque_controller_min_max_dq " << dqMinMaxFromConf.size() << " is not correct number " << 2 * m_robot->numJoints() << ". Use default values." << std::endl; dq_min_max_from_conf_is_valid = false; } - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_motorTorqueControllers[i].setErrorPrefix(std::string(m_profile.instance_name)); m_motorTorqueControllers[i].setupMotorControllerTransitionMinMaxDq(m_robot->joint(i)->lvlimit * m_dt, m_robot->joint(i)->uvlimit * m_dt); if(dq_min_max_from_conf_is_valid) { @@ -285,7 +285,7 @@ RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id) m_qCurrentIn.data.length() == m_robot->numJoints()) { // update model - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qCurrentIn.data[i]; } m_robot->calcForwardKinematics(); @@ -294,7 +294,7 @@ RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id) executeTorqueControl(dq); // check range of motion and insert to output - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { if (m_motorTorqueControllers[i].isEnabled()) { m_qRefOut.data[i] = std::min(std::max(m_qRefIn.data[i] + dq[i], m_robot->joint(i)->llimit), m_robot->joint(i)->ulimit); } else { @@ -311,7 +311,7 @@ RTC::ReturnCode_t TorqueController::onExecute(RTC::UniqueId ec_id) std::cerr << std::endl; } // pass qRefIn to qRefOut - for (int i = 0; i < m_robot->numJoints(); i++) { + for (unsigned int i = 0; i < m_robot->numJoints(); i++) { m_qRefOut.data[i] = m_qRefIn.data[i]; } } @@ -366,12 +366,12 @@ RTC::ReturnCode_t TorqueController::onRateChanged(RTC::UniqueId ec_id) void TorqueController::executeTorqueControl(hrp::dvector &dq) { - int numJoints = m_robot->numJoints(); + unsigned int numJoints = m_robot->numJoints(); hrp::dvector tauMax(numJoints); dq.resize(numJoints); // determine tauMax - for(int i = 0; i < numJoints; i++) { + for(unsigned int i = 0; i < numJoints; i++) { double tauMaxFromModel = m_robot->joint(i)->climit * m_robot->joint(i)->gearRatio * m_robot->joint(i)->torqueConst; if ( m_tauMaxIn.data.length() == m_robot->numJoints() ) { tauMax[i] = std::min(tauMaxFromModel, m_tauMaxIn.data[i]); @@ -384,19 +384,19 @@ void TorqueController::executeTorqueControl(hrp::dvector &dq) // tauCurrent.length is assumed to be equal to numJoints (check in onExecute) if (isDebug()) { std::cerr << "[" << m_profile.instance_name << "]" << "tauCurrentIn: "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << m_tauCurrentIn.data[i]; } std::cerr << std::endl; std::cerr << "[" << m_profile.instance_name << "]" << "tauMax: "; - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { std::cerr << " " << tauMax[i]; } std::cerr << std::endl; } Guard guard(m_mutex); - for (int i = 0; i < numJoints; i++) { + for (unsigned int i = 0; i < numJoints; i++) { dq[i] = m_motorTorqueControllers[i].execute(m_tauCurrentIn.data[i], tauMax[i]); // twoDofController: tau = -K(q - qRef) // output debug message if (isDebug() && m_motorTorqueControllers[i].getMotorControllerState() != MotorTorqueController::INACTIVE) { @@ -434,7 +434,7 @@ bool TorqueController::enableMultipleTorqueControllers(const OpenHRP::TorqueCont { bool succeed = true; bool retval; - for (int i = 0; i < jnames.length(); i++) { + for (unsigned int i = 0; i < jnames.length(); i++) { retval = enableTorqueController(std::string(jnames[i])); if (!retval) { // return false when once failed succeed = false; @@ -461,7 +461,7 @@ bool TorqueController::disableMultipleTorqueControllers(const OpenHRP::TorqueCon { bool succeed = true; bool retval; - for (int i = 0; i < jnames.length(); i++) { + for (unsigned int i = 0; i < jnames.length(); i++) { retval = disableTorqueController(std::string(jnames[i])); if (!retval) { // return false when once failed succeed = false; @@ -497,7 +497,7 @@ bool TorqueController::startMultipleTorqueControls(const OpenHRP::TorqueControll { bool succeed = true; bool retval; - for (int i = 0; i < jnames.length(); i++) { + for (unsigned int i = 0; i < jnames.length(); i++) { retval = startTorqueControl(std::string(jnames[i])); if (!retval) { // return false when once failed succeed = false; @@ -524,7 +524,7 @@ bool TorqueController::stopMultipleTorqueControls(const OpenHRP::TorqueControlle { bool succeed = true; bool retval; - for (int i = 0; i < jnames.length(); i++) { + for (unsigned int i = 0; i < jnames.length(); i++) { retval = stopTorqueControl(std::string(jnames[i])); if (!retval) { // return false when once failed succeed = false; @@ -562,7 +562,7 @@ bool TorqueController::setMultipleReferenceTorques(const OpenHRP::TorqueControll return false; } // set reference torques - for (int i = 0; i < jnames.length(); i++) { + for (unsigned int i = 0; i < jnames.length(); i++) { retval = setReferenceTorque(std::string(jnames[i]), tauRefs[i]); if (!retval) { // return false when once failed succeed = false; diff --git a/rtc/TorqueController/TorqueController.h b/rtc/TorqueController/TorqueController.h index e3ea6445155..5e4c122841d 100644 --- a/rtc/TorqueController/TorqueController.h +++ b/rtc/TorqueController/TorqueController.h @@ -10,6 +10,7 @@ #ifndef TORQUE_CONTROLLER_H #define TPRQUE_CONTROLLER_H +#include #include #include #include diff --git a/rtc/TorqueController/TorqueControllerService_impl.h b/rtc/TorqueController/TorqueControllerService_impl.h index e83fab755bd..49d46f103a6 100644 --- a/rtc/TorqueController/TorqueControllerService_impl.h +++ b/rtc/TorqueController/TorqueControllerService_impl.h @@ -2,7 +2,7 @@ #ifndef __NULL_SERVICE_H__ #define __NULL_SERVICE_H__ -#include "TorqueControllerService.hh" +#include "hrpsys/idl/TorqueControllerService.hh" using namespace OpenHRP; diff --git a/rtc/TorqueFilter/CMakeLists.txt b/rtc/TorqueFilter/CMakeLists.txt index d8c839a5738..10bf49f3c5f 100644 --- a/rtc/TorqueFilter/CMakeLists.txt +++ b/rtc/TorqueFilter/CMakeLists.txt @@ -16,6 +16,6 @@ add_test(testIIRFilterDoubleTest0 testIIRFilter --double --test0 --use-gnuplot f add_test(testIIRFilterVector3Test0 testIIRFilter --double --test0 --use-gnuplot false) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/TorqueFilter/IIRFilter.cpp b/rtc/TorqueFilter/IIRFilter.cpp index 65e82cbcf19..886e29cb68a 100644 --- a/rtc/TorqueFilter/IIRFilter.cpp +++ b/rtc/TorqueFilter/IIRFilter.cpp @@ -1,48 +1,132 @@ -#include #include "IIRFilter.h" +#include -IIRFilter::IIRFilter(int dim, std::vector& fb_coeffs, std::vector& ff_coeffs, const std::string& error_prefix) +IIRFilter::IIRFilter(unsigned int dim, std::vector& fb_coeffs, std::vector& ff_coeffs, const std::string& error_prefix) { - // init dimention - m_dimention = dim; - - // init coefficients - if(fb_coeffs.size() != dim + 1|| ff_coeffs.size() != dim + 1){ - std::cout << "[" << error_prefix << "]" << "IIRFilter coefficients size error" << std::endl; + std::cerr << "This IIRFilter constructure is obsolated method." << std::endl; + m_dimension = dim; + m_error_prefix = error_prefix; + + // init coefficients + if(fb_coeffs.size() != dim + 1|| ff_coeffs.size() != dim + 1){ + std::cout << "[" << m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl; + return; + } + for(std::vector::iterator it = fb_coeffs.begin(); it != fb_coeffs.end(); it++){ + m_fb_coefficients.push_back(*it); + } + for(std::vector::iterator it = ff_coeffs.begin(); it != ff_coeffs.end(); it++){ + m_ff_coefficients.push_back(*it); + } + + // init previous values + m_previous_values.assign(dim, 0.0); + m_initialized = true; return; - } - for(std::vector::iterator it = fb_coeffs.begin(); it != fb_coeffs.end(); it++){ - m_fb_coefficients.push_back(*it); - } - for(std::vector::iterator it = ff_coeffs.begin(); it != ff_coeffs.end(); it++){ - m_ff_coefficients.push_back(*it); - } - - // init previous values - m_previous_values.assign(dim, 0.0); - - return; } -IIRFilter::~IIRFilter() -{ +IIRFilter::IIRFilter(const std::string& error_prefix) : + m_initialized(false) { + m_error_prefix = error_prefix; +} + +bool IIRFilter::setParameter(int dim, std::vector& A, std::vector& B) { + m_dimension = dim; + + // init coefficients + if((A.size() != dim && A.size() != dim + 1) || B.size() != dim + 1) { + std::cout << "[" << m_error_prefix << "]" << "IIRFilter coefficients size error" << std::endl; + return false; + } + + // clear previous coefficients + m_fb_coefficients.clear(); + m_ff_coefficients.clear(); + + if (A.size() == dim) { + m_fb_coefficients.push_back(1.0); + } + for(std::vector::iterator it = A.begin(); it != A.end(); it++){ + if (it == A.begin()) { + if( *it != 1.0 ) { + std::cout << "[" << m_error_prefix << "]" << "IIRFilter : parameter A[0] is not 1.0 !!!" << std::endl; + } + m_fb_coefficients.push_back(*it); + } else { + m_fb_coefficients.push_back(- *it); + } + } + for(std::vector::iterator it = B.begin(); it != B.end(); it++){ + m_ff_coefficients.push_back(*it); + } + + // init previous values + m_previous_values.assign(dim, 0.0); + m_initialized = true; + return true; +} + +bool IIRFilter::setParameterAsBiquad(const double f_cutoff, const double Q, const double hz){ + std::vector fb_coeffs(3), ff_coeffs(3); + const double omega = 2 * 3.14159265 * f_cutoff / hz; + const double alpha = std::sin(omega) / (2 * Q); + const double denom = 1 + alpha; + fb_coeffs[0] = 1; + fb_coeffs[1] = -2 * std::cos(omega) / denom; + fb_coeffs[2] = (1 - alpha) / denom; + ff_coeffs[0] = (1 - std::cos(omega)) / 2 / denom; + ff_coeffs[1] = (1 - std::cos(omega)) / denom; + ff_coeffs[2] = (1 - std::cos(omega)) / 2 / denom; + return this->setParameter(2, fb_coeffs, ff_coeffs); }; -double IIRFilter::executeFilter(double input) + +void IIRFilter::getParameter(int &dim, std::vector& A, std::vector& B) +{ + dim = m_dimension; + B.resize(m_ff_coefficients.size()); + std::copy(m_ff_coefficients.begin(), m_ff_coefficients.end(), B.begin()); + A.resize(0); + for(std::vector::iterator it = m_fb_coefficients.begin(); + it != m_fb_coefficients.end(); it++) { + if (it == m_fb_coefficients.begin()) { + A.push_back(*it); + } else { + A.push_back(- *it); + } + } +} + +void IIRFilter::reset(double initial_input) { - double feedback, filtered; - // calcurate retval - feedback = m_fb_coefficients[0] * input; - for (int i = 0; i < m_dimention; i++){ - feedback += m_fb_coefficients[i + 1] * m_previous_values[i]; - } - filtered = m_ff_coefficients[0] * feedback; - for(int i = 0; i < m_dimention; i++){ - filtered += m_ff_coefficients[i + 1] * m_previous_values[i]; - } - // update previous values - m_previous_values.push_front(feedback); - m_previous_values.pop_back(); - - return filtered; + // y[n] = b[0]*w[n] + b[1]*w[n-1] + ... + b[m]*w[n-m] in DirectForm-II. + // When n->inf, y[n]->initial_input and w[n], w[n-1], ..., w[n-m] -> w, + // m_previous_values should preserve w + double sum_ff_coeffs = std::accumulate(m_ff_coefficients.begin(), m_ff_coefficients.end(), 0.0); + double reset_val = initial_input / sum_ff_coeffs; + m_previous_values.assign(m_dimension, reset_val); +} + +double IIRFilter::passFilter(double input) +{ + // IIRFilter implementation based on DirectForm-II. + // Cf. https://en.wikipedia.org/wiki/Digital_filter + if (! m_initialized) { + return 0.0; + } + double feedback, filtered; + // calcurate retval + feedback = m_fb_coefficients[0] * input; + for (int i = 0; i < m_dimension; i++) { + feedback += m_fb_coefficients[i + 1] * m_previous_values[i]; + } + filtered = m_ff_coefficients[0] * feedback; + for(int i = 0; i < m_dimension; i++) { + filtered += m_ff_coefficients[i + 1] * m_previous_values[i]; + } + // update previous values + m_previous_values.push_front(feedback); + m_previous_values.pop_back(); + + return filtered; } diff --git a/rtc/TorqueFilter/IIRFilter.h b/rtc/TorqueFilter/IIRFilter.h index f453da11adc..ecdd7b8c8ae 100644 --- a/rtc/TorqueFilter/IIRFilter.h +++ b/rtc/TorqueFilter/IIRFilter.h @@ -15,6 +15,7 @@ #include #include #include +#include /** Infinite Impulse Filter @@ -22,33 +23,72 @@ */ class IIRFilter { - public: - - /** - \brief Constructor - \param dim dimention of the filter - \param fb_coeffs coeeficients of feedback - \param ff_coeffs coefficients of feedforward - */ - IIRFilter(int dim, std::vector& fb_coeffs, std::vector& ff_coeffs, const std::string& error_prefix = ""); - /** - \brief Destructor - */ - ~IIRFilter(); +public: + /** + \brief Constructor + */ + IIRFilter(const std::string& error_prefix = ""); + /** + \brief Constructor, this constructure will be obsolated + \param dim dimension of the filter + \param fb_coeffs coeeficients of feedback + \param ff_coeffs coefficients of feedforward + */ + IIRFilter(unsigned int dim, std::vector& fb_coeffs, std::vector& ff_coeffs, const std::string& error_prefix = ""); + /** + \brief Destructor + */ + ~IIRFilter() {}; + + /** + \brief Set parameters + Y[n] = B[0] * X[n] + B[1] * X[n-1] + ... + B[dim] * X[n-dim] - A[1] * Y[n-1] ... - A[dim] * Y[n-dim] + A[0] would be 1.0 + + How to generete parameter by octave + butterworth filter (dimension = 2, cutoff_freq = 8Hz) + [B, A] = butter(2, 2 * 0.004 * 8) ;;; dimension=2, 2 * dt * cutoff_freq + */ + bool setParameter(int dim, std::vector& A, std::vector& B); - /** - \brief Execute filtering - */ - double executeFilter(double input); + /** + \brief Simple user interface of setParameter + \param f_cutoff cut off frequency + \param Q quality factor: 1/2 = no overshoot, 1/sqrt(2) = Butterworth + \param hz sampling rate + */ + bool setParameterAsBiquad(const double f_cutoff, const double Q, const double hz); - private: - // Configuration variable declaration - // - int m_dimention; - std::vector m_fb_coefficients; // fb parameters (dim must be m_dimention + 1, m_fb_coefficients[0] would be 1.0) - std::vector m_ff_coefficients; // ff parameters (dim must be m_dimention + 1) - std::deque m_previous_values; + /** + */ + void getParameter(int &dim, std::vector&A, std::vector& B); + /** + */ + void reset(double initial_input = 0.0); + + /** + \brief Execute filtering, this method will be obsolated + */ + double executeFilter(double input) { + // std::cerr << "executeFilter will be obsolated." << std::endl; + return passFilter(input); + }; + /** + \brief passFilter + */ + double passFilter(double input); + // double getCurrentValue () const { return m_prev_output; }; +private: + // Configuration variable declaration + // + int m_dimension; + std::vector m_fb_coefficients; // fb parameters (dim must be m_dimension + 1, m_fb_coefficients[0] would be 1.0) + std::vector m_ff_coefficients; // ff parameters (dim must be m_dimension + 1) + std::deque m_previous_values; + // double m_prev_output; + bool m_initialized; + std::string m_error_prefix; }; /** @@ -60,26 +100,26 @@ template class FirstOrderLowPassFilter T prev_value; double cutoff_freq, dt, const_param; public: - FirstOrderLowPassFilter (const double _cutoff_freq, const double _dt, const T init_value) : dt(_dt), prev_value(init_value) + FirstOrderLowPassFilter (const double _cutoff_freq, const double _dt, const T init_value) : prev_value(init_value), dt(_dt) { setCutOffFreq(_cutoff_freq); }; ~FirstOrderLowPassFilter() { }; - T passFilter (T value) + T passFilter (const T& value) { prev_value = 1.0/(1+const_param) * prev_value + const_param/(1+const_param) * value; return prev_value; }; - void reset (T value) { prev_value = value; }; + void reset (const T& value) { prev_value = value; }; void setCutOffFreq (const double f) { cutoff_freq = f; const_param = 2 * M_PI * cutoff_freq * dt; }; double getCutOffFreq () const { return cutoff_freq; }; - double getCurrentValue () const { return prev_value; }; + T getCurrentValue () const { return prev_value; }; }; #endif // IIRFilter_H diff --git a/rtc/TorqueFilter/TorqueFilter.cpp b/rtc/TorqueFilter/TorqueFilter.cpp index 961c3aa0d79..f108fa0ae3d 100644 --- a/rtc/TorqueFilter/TorqueFilter.cpp +++ b/rtc/TorqueFilter/TorqueFilter.cpp @@ -125,7 +125,7 @@ RTC::ReturnCode_t TorqueFilter::onInitialize() m_torque_offset.resize(m_robot->numJoints()); coil::vstring torque_offset = coil::split(prop["torque_offset"], ","); if ( m_torque_offset.size() == torque_offset.size() && m_debugLevel > 0) { - for(int i = 0; i < m_robot->numJoints(); i++){ + for(unsigned int i = 0; i < m_robot->numJoints(); i++){ coil::stringTo(m_torque_offset[i], torque_offset[i].c_str()); std::cerr << "[" << m_profile.instance_name << "]" << "offset[" << m_robot->joint(i)->name << "]: " << m_torque_offset[i] << std::endl; } @@ -156,7 +156,7 @@ RTC::ReturnCode_t TorqueFilter::onInitialize() std::cerr<< "[" << m_profile.instance_name << "]" << "There is no torque_filter_params. Use default values." << std::endl; } } - if (!use_default_flag && ((filter_dim + 1) * 2 + 1 != torque_filter_params.size()) ) { + if (!use_default_flag && ((filter_dim + 1) * 2 + 1 != (int)torque_filter_params.size()) ) { if (m_debugLevel > 0) { std::cerr<< "[" << m_profile.instance_name << "]" << "Size of torque_filter_params is not correct. Use default values." << std::endl; } @@ -198,7 +198,7 @@ RTC::ReturnCode_t TorqueFilter::onInitialize() } // make filter instance - for(int i = 0; i < m_robot->numJoints(); i++){ + for(unsigned int i = 0; i < m_robot->numJoints(); i++){ m_filters.push_back(IIRFilter(filter_dim, fb_coeffs, ff_coeffs, std::string(m_profile.instance_name))); } @@ -265,7 +265,7 @@ RTC::ReturnCode_t TorqueFilter::onExecute(RTC::UniqueId ec_id) if (m_qCurrent.data.length() == m_robot->numJoints()) { // reference robot model - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qCurrent.data[i]; } m_robot->calcForwardKinematics(); diff --git a/rtc/TorqueFilter/TorqueFilter.h b/rtc/TorqueFilter/TorqueFilter.h index 63cdc2d6e4c..99c65aee948 100644 --- a/rtc/TorqueFilter/TorqueFilter.h +++ b/rtc/TorqueFilter/TorqueFilter.h @@ -10,6 +10,7 @@ #ifndef TORQUE_FILTER_H #define TORQUE_FILTER_H +#include #include #include #include diff --git a/rtc/TorqueFilter/testIIRFilter.cpp b/rtc/TorqueFilter/testIIRFilter.cpp index 4a813f0ceb2..93dc8f0578e 100644 --- a/rtc/TorqueFilter/testIIRFilter.cpp +++ b/rtc/TorqueFilter/testIIRFilter.cpp @@ -9,13 +9,13 @@ #include #include -template +template class testIIRFilter { protected: double dt; /* [s] */ double input_freq; /* [Hz] */ - boost::shared_ptr > filter; + boost::shared_ptr filter; bool use_gnuplot; void fprintf_value (FILE* fp, const double _time, const T& _input, const T& _output); void fprintf_plot (FILE* gp_pos); @@ -45,8 +45,10 @@ class testIIRFilter public: std::vector arg_strs; testIIRFilter (const double _dt = 0.004) : dt(_dt), input_freq(1.0), - filter(boost::shared_ptr >(new FirstOrderLowPassFilter(4.0, dt, init_value()))), - use_gnuplot(true) {}; + use_gnuplot(true) { initialize(); }; + void initialize() { + filter = boost::shared_ptr(new FT (4.0, dt, init_value())); + } void test0 () { std::cerr << "test0 : test" << std::endl; @@ -78,34 +80,94 @@ class testIIRFilter }; // Specialization for double -template<> void testIIRFilter::fprintf_value (FILE* fp, const double _time, const double& _input, const double& _output) +template<> void testIIRFilter >::fprintf_value (FILE* fp, const double _time, const double& _input, const double& _output) { fprintf(fp, "%f %f %f\n", _time, _input, _output); }; -template<> double testIIRFilter::init_value () { return 0;}; -template<> double testIIRFilter::test0_input_value (const size_t i) { return calc_sin_value(i);}; -template<> void testIIRFilter::fprintf_plot (FILE* gp_pos) +template<> double testIIRFilter >::init_value () { return 0;}; +template<> double testIIRFilter >::test0_input_value (const size_t i) { return calc_sin_value(i);}; +template<> void testIIRFilter >::fprintf_plot (FILE* gp_pos) { fprintf(gp_pos, "plot '/tmp/plot-iirfilter.dat' using 1:2 with lines title 'input' lw 4, '/tmp/plot-iirfilter.dat' using 1:3 with lines title 'filtered' lw 3\n"); }; // Specialization for hrp::Vector3 -template<> void testIIRFilter::fprintf_value (FILE* fp, const double _time, const hrp::Vector3& _input, const hrp::Vector3& _output) +template<> void testIIRFilter >::fprintf_value (FILE* fp, const double _time, const hrp::Vector3& _input, const hrp::Vector3& _output) { fprintf(fp, "%f %f %f %f %f %f %f\n", _time, _input[0], _output[0], _input[1], _output[1], _input[2], _output[2]); }; -template<> hrp::Vector3 testIIRFilter::init_value () { return hrp::Vector3::Zero();}; -template<> hrp::Vector3 testIIRFilter::test0_input_value (const size_t i) +template<> hrp::Vector3 testIIRFilter >::init_value () { return hrp::Vector3::Zero();}; +template<> hrp::Vector3 testIIRFilter >::test0_input_value (const size_t i) { double tmp = calc_sin_value(i); return hrp::Vector3(tmp, 2*tmp, -0.5*tmp); }; -template<> void testIIRFilter::fprintf_plot (FILE* gp_pos) +template<> void testIIRFilter >::fprintf_plot (FILE* gp_pos) { fprintf(gp_pos, "plot '/tmp/plot-iirfilter.dat' using 1:2 with lines title 'input (0)' lw 4, '/tmp/plot-iirfilter.dat' using 1:3 with lines title 'filtered (0)' lw 3,"); fprintf(gp_pos, "'/tmp/plot-iirfilter.dat' using 1:4 with lines title 'input (1)' lw 4, '/tmp/plot-iirfilter.dat' using 1:5 with lines title 'filtered (1)' lw 3,"); fprintf(gp_pos, "'/tmp/plot-iirfilter.dat' using 1:6 with lines title 'input (2)' lw 4, '/tmp/plot-iirfilter.dat' using 1:7 with lines title 'filtered (2)' lw 3\n"); }; +/// +template<> void testIIRFilter::initialize () { +#if 0 // use obsolated method + int filter_dim = 0; + std::vector fb_coeffs, ff_coeffs; + filter_dim = 2; + fb_coeffs.resize(filter_dim+1); + fb_coeffs[0] = 1.00000; + fb_coeffs[1] = 1.88903; + fb_coeffs[2] =-0.89487; + ff_coeffs.resize(filter_dim+1); + ff_coeffs[0] = 0.0014603; + ff_coeffs[1] = 0.0029206; + ff_coeffs[2] = 0.0014603; + filter = boost::shared_ptr(new IIRFilter (filter_dim, fb_coeffs, ff_coeffs)); +#else + filter = boost::shared_ptr(new IIRFilter ()); + int dim = 2; + std::vector A(dim+1); + std::vector B(dim+1); + // octave + // [B, A] = butter(2, 0.004 * 2 * 8) ;;; 2 * dt * cutoff_freq + //b = + //0.00882608666843131 0.01765217333686262 0.00882608666843131 + //a = + //1.000000000000000 -1.717211834908084 0.752516181581809 + A[0] = 1.000000000000000; + A[1] = -1.717211834908084; + A[2] = 0.752516181581809; + B[0] = 0.00882608666843131; + B[1] = 0.01765217333686262; + B[2] = 0.00882608666843131; + filter->setParameter(dim, A, B); +#endif +}; +template<> void testIIRFilter::parse_params () { + for (int i = 0; i < arg_strs.size(); ++ i) { + if ( arg_strs[i]== "--use-gnuplot" ) { + if (++i < arg_strs.size()) use_gnuplot = (arg_strs[i]=="true"); +#if 0 + } else if ( arg_strs[i]== "--cutoff-freq" ) { + if (++i < arg_strs.size()) filter->setCutOffFreq(atof(arg_strs[i].c_str())); +#endif + } else if ( arg_strs[i]== "--input-freq" ) { + if (++i < arg_strs.size()) input_freq = atof(arg_strs[i].c_str()); + } + } + std::cerr << "[testIIRFilter] params" << std::endl; + std::cerr << "[testIIRFilter] dt = " << dt << "[s], cutoff-freq = " << 8 << "[Hz], input-freq = " << input_freq << "[Hz]" << std::endl; +}; +template<> void testIIRFilter::fprintf_value (FILE* fp, const double _time, const double& _input, const double& _output) +{ + fprintf(fp, "%f %f %f\n", _time, _input, _output); +}; +template<> double testIIRFilter::init_value () { return 0;}; +template<> double testIIRFilter::test0_input_value (const size_t i) { return calc_sin_value(i);}; +template<> void testIIRFilter::fprintf_plot (FILE* gp_pos) +{ + fprintf(gp_pos, "plot '/tmp/plot-iirfilter.dat' using 1:2 with lines title 'input' lw 4, '/tmp/plot-iirfilter.dat' using 1:3 with lines title 'filtered' lw 3\n"); +}; void print_usage () { @@ -121,7 +183,7 @@ int main(int argc, char* argv[]) int ret = 0; if (argc >= 3) { if (std::string(argv[1]) == "--double") { - testIIRFilter tiir; + testIIRFilter > tiir; for (int i = 2; i < argc; ++ i) { tiir.arg_strs.push_back(std::string(argv[i])); } @@ -132,7 +194,18 @@ int main(int argc, char* argv[]) ret = 1; } } else if (std::string(argv[1]) == "--vector3") { - testIIRFilter tiir; + testIIRFilter > tiir; + for (int i = 2; i < argc; ++ i) { + tiir.arg_strs.push_back(std::string(argv[i])); + } + if (std::string(argv[2]) == "--test0") { + tiir.test0(); + } else { + print_usage(); + ret = 1; + } + } else if (std::string(argv[1]) == "--iir") { + testIIRFilter tiir; for (int i = 2; i < argc; ++ i) { tiir.arg_strs.push_back(std::string(argv[i])); } diff --git a/rtc/UndistortImage/CMakeLists.txt b/rtc/UndistortImage/CMakeLists.txt index aaf3e5c946b..bf41f46c68c 100644 --- a/rtc/UndistortImage/CMakeLists.txt +++ b/rtc/UndistortImage/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(UndistortImageComp ${libs}) set(target UndistortImage UndistortImageComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/UndistortImage/UndistortImage.cpp b/rtc/UndistortImage/UndistortImage.cpp index acd9ddb45d6..abedbe6adcb 100644 --- a/rtc/UndistortImage/UndistortImage.cpp +++ b/rtc/UndistortImage/UndistortImage.cpp @@ -172,7 +172,7 @@ RTC::ReturnCode_t UndistortImage::onExecute(RTC::UniqueId ec_id) { // RGB -> BGR char *dst = m_cvImage->imageData; - for (int i=0; i RGB char *src = dst_img->imageData; - for (int i=0; i #include #include #include @@ -18,7 +20,6 @@ #include #include #include -#include "Img.hh" // Service implementation headers // diff --git a/rtc/VideoCapture/CMakeLists.txt b/rtc/VideoCapture/CMakeLists.txt index 21edfb04812..52ea4f0aeb0 100644 --- a/rtc/VideoCapture/CMakeLists.txt +++ b/rtc/VideoCapture/CMakeLists.txt @@ -13,6 +13,6 @@ target_link_libraries(testCamera ${OpenCV_LIBRARIES}) set(target VideoCapture VideoCaptureComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/VideoCapture/CameraCaptureService_impl.h b/rtc/VideoCapture/CameraCaptureService_impl.h index 6624cb13a7b..c0340df1dac 100644 --- a/rtc/VideoCapture/CameraCaptureService_impl.h +++ b/rtc/VideoCapture/CameraCaptureService_impl.h @@ -2,7 +2,7 @@ #ifndef __CAMERA_CAPTURE_SERVICE_H__ #define __CAMERA_CAPTURE_SERVICE_H__ -#include "Img.hh" +#include "hrpsys/idl/Img.hh" class VideoCapture; diff --git a/rtc/VideoCapture/VideoCapture.cpp b/rtc/VideoCapture/VideoCapture.cpp index 91f9ee884ea..469c2418a91 100644 --- a/rtc/VideoCapture/VideoCapture.cpp +++ b/rtc/VideoCapture/VideoCapture.cpp @@ -7,7 +7,7 @@ * $Id$ */ -#include "util/VectorConvert.h" +#include "hrpsys/util/VectorConvert.h" #include "VideoCapture.h" // Module specification @@ -43,7 +43,8 @@ VideoCapture::VideoCapture(RTC::Manager* manager) m_CameraCaptureServicePort("CameraCaptureService"), m_CameraCaptureService(this), // - m_mode(CONTINUOUS) + m_mode(CONTINUOUS), + m_needToReactivate(false) { } @@ -163,7 +164,17 @@ RTC::ReturnCode_t VideoCapture::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t VideoCapture::onExecute(RTC::UniqueId ec_id) { //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; - capture(); + if (m_needToReactivate){ + if (onActivated(ec_id) == RTC::RTC_OK){ + m_needToReactivate = false; + } + } + if (!capture()){ + std::cerr << m_profile.instance_name << ": failed to capture." << std::endl; + onDeactivated(ec_id); + m_needToReactivate = true; + return RTC::RTC_OK; + } double tNew = (double)(coil::gettimeofday()); double dt = (double)(tNew - m_tOld); @@ -186,23 +197,25 @@ RTC::ReturnCode_t VideoCapture::onExecute(RTC::UniqueId ec_id) return RTC::RTC_OK; } -void VideoCapture::capture() +bool VideoCapture::capture() { if (m_cameras.size() == 1){ m_CameraImage.error_code = 0; uchar *imgFrom = m_cameras[0]->capture(); + if (!imgFrom) return false; memcpy(m_CameraImage.data.image.raw_data.get_buffer(), imgFrom, m_CameraImage.data.image.raw_data.length() * sizeof(uchar)); - return; }else{ m_MultiCameraImages.error_code = 0; for (unsigned int i = 0; i < m_cameras.size (); i++) { uchar *imgFrom = m_cameras[i]->capture(); + if (!imgFrom) return false; memcpy (m_MultiCameraImages.data.image_seq[i].image.raw_data.get_buffer(), imgFrom, m_MultiCameraImages.data.image_seq[i].image.raw_data.length() * sizeof (uchar)); } } + return true; } /* diff --git a/rtc/VideoCapture/VideoCapture.h b/rtc/VideoCapture/VideoCapture.h index 0db2e12e84a..fd65bb337ec 100644 --- a/rtc/VideoCapture/VideoCapture.h +++ b/rtc/VideoCapture/VideoCapture.h @@ -10,13 +10,14 @@ #ifndef VIDEO_CAPTURE_H #define VIDEO_CAPTURE_H +#include +#include "hrpsys/idl/Img.hh" #include #include #include #include #include #include -#include "Img.hh" #include "camera.h" // Service implementation headers @@ -97,7 +98,7 @@ class VideoCapture // no corresponding operation exists in OpenRTm-aist-0.2.0 // virtual RTC::ReturnCode_t onRateChanged(RTC::UniqueId ec_id); - void capture(); + bool capture(); void take_one_frame(); void start_continuous(); void stop_continuous(); @@ -147,6 +148,7 @@ class VideoCapture std::vector < v4l_capture * > m_cameras; int m_width, m_height, m_frameRate; double m_tOld; + bool m_needToReactivate; }; diff --git a/rtc/VideoCapture/camera.cpp b/rtc/VideoCapture/camera.cpp index 7b5afb8d42d..c0b9aa6c881 100644 --- a/rtc/VideoCapture/camera.cpp +++ b/rtc/VideoCapture/camera.cpp @@ -32,7 +32,7 @@ v4l_capture::init(size_t _width, size_t _height, unsigned int devId) uchar * v4l_capture::capture () { - write_img(frame.data); + if (!write_img(frame.data)) return NULL; return frame.data; } @@ -73,18 +73,19 @@ bool v4l_capture::open_device(void) return true; } -void v4l_capture::close_device(void) +bool v4l_capture::close_device(void) { if (close(fd) == -1) { perror("close"); - exit(EXIT_FAILURE); + return false; } fd = -1; + return true; } -void v4l_capture::write_img(uchar* ret) +bool v4l_capture::write_img(uchar* ret) { - read_frame(); + if (!read_frame()) return false; for (int i = 0; i < width * height; i += 2) { int y, r, g, b; @@ -109,9 +110,11 @@ void v4l_capture::write_img(uchar* ret) ret[(i + 1) * 3 + 1] = (unsigned char) (std::min(std::max(g, 0), 255)); ret[(i + 1) * 3 + 2] = (unsigned char) (std::min(std::max(b, 0), 255)); } + + return true; } -void v4l_capture::read_frame(void) +bool v4l_capture::read_frame(void) { struct v4l2_buffer buf; @@ -122,17 +125,19 @@ void v4l_capture::read_frame(void) if (ioctl(fd, VIDIOC_DQBUF, &buf) == -1) { perror("VIDIOC_DQBUF"); - exit(EXIT_FAILURE); + return false; } assert(buf.index < n_buffers); if (ioctl(fd, VIDIOC_QBUF, &buf) == -1) { perror("VIDIOC_QBUF"); - exit(EXIT_FAILURE); + return false; } + + return true; } -void v4l_capture::init_mmap(void) +bool v4l_capture::init_mmap(void) { struct v4l2_requestbuffers req; @@ -144,19 +149,19 @@ void v4l_capture::init_mmap(void) if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1) { perror("VIDIOC_REQBUFS"); - exit(EXIT_FAILURE); + return false; } if (req.count < 2) { fprintf(stderr, "Insufficient buffer memory on %s\n", dev_name.c_str()); - exit(EXIT_FAILURE); + return false; } buffers = (buffer*)calloc(req.count, sizeof(*buffers)); if (!buffers) { fprintf(stderr, "Out of memory\n"); - exit(EXIT_FAILURE); + return false; } for (n_buffers = 0; n_buffers < req.count; ++n_buffers) { @@ -170,7 +175,7 @@ void v4l_capture::init_mmap(void) if (ioctl(fd, VIDIOC_QUERYBUF, &buf) == -1) { perror("VIDIOC_QUERYBUF"); - exit(EXIT_FAILURE); + return false; } buffers[n_buffers].length = buf.length; @@ -182,24 +187,26 @@ void v4l_capture::init_mmap(void) if (buffers[n_buffers].start == MAP_FAILED) { perror("mmap"); - exit(EXIT_FAILURE); + return false; } } + return true; } -void v4l_capture::uninit_mmap(void) +bool v4l_capture::uninit_mmap(void) { unsigned int i; for (i = 0; i < n_buffers; ++i) { if (munmap(buffers[i].start, buffers[i].length) == -1) { perror("munmap"); - exit(EXIT_FAILURE); + return false; } } + return true; } -void v4l_capture::init_device(void) +bool v4l_capture::init_device(void) { struct v4l2_capability cap; struct v4l2_format fmt; @@ -209,7 +216,7 @@ void v4l_capture::init_device(void) fprintf(stderr, "%s is no V4L2 device\n", dev_name.c_str()); } perror("VIDIOC_QUERYCAP"); - exit(EXIT_FAILURE); + return false; } fprintf(stderr, "video capabilities\n"); @@ -258,7 +265,7 @@ void v4l_capture::init_device(void) if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) { fprintf(stderr, "%s is no video capture device\n", dev_name.c_str()); - exit(EXIT_FAILURE); + return false; } memset (&(fmt), 0, sizeof (fmt)); @@ -271,10 +278,11 @@ void v4l_capture::init_device(void) if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1) { perror("VIDIOC_S_FMT"); - exit(EXIT_FAILURE); + return false; } init_mmap(); + return true; } void v4l_capture::uninit_device(void) @@ -312,7 +320,7 @@ bool v4l_capture::start_capturing(void) return true; } -void v4l_capture::stop_capturing(void) +bool v4l_capture::stop_capturing(void) { enum v4l2_buf_type type; @@ -320,7 +328,7 @@ void v4l_capture::stop_capturing(void) if (ioctl(fd, VIDIOC_STREAMOFF, &type) == -1) { perror("VIDIOC_STREAMOFF"); - exit(EXIT_FAILURE); + return false; } - + return true; } diff --git a/rtc/VideoCapture/camera.h b/rtc/VideoCapture/camera.h index fbfc0c80b91..4ea2b8d7682 100644 --- a/rtc/VideoCapture/camera.h +++ b/rtc/VideoCapture/camera.h @@ -20,15 +20,15 @@ class v4l_capture buffer *buffers; unsigned int n_buffers; bool open_device(); - void init_device(); - void init_mmap(); + bool init_device(); + bool init_mmap(); bool start_capturing(); - void stop_capturing(); + bool stop_capturing(); void uninit_device(); - void uninit_mmap(); - void close_device(); - void read_frame(void); - void write_img(uchar * ret); + bool uninit_mmap(); + bool close_device(); + bool read_frame(void); + bool write_img(uchar * ret); bool init_all(size_t _width, size_t _height, unsigned int _devId); public: v4l_capture(); diff --git a/rtc/VideoCapture/testCamera.cpp b/rtc/VideoCapture/testCamera.cpp index e5e8fce3b14..99ddee828d5 100644 --- a/rtc/VideoCapture/testCamera.cpp +++ b/rtc/VideoCapture/testCamera.cpp @@ -1,5 +1,6 @@ #include "camera.h" #include +#include int main(int argc, char *argv[]) { diff --git a/rtc/Viewer/CMakeLists.txt b/rtc/Viewer/CMakeLists.txt index 6e8b3b5bfaa..4edd424beea 100644 --- a/rtc/Viewer/CMakeLists.txt +++ b/rtc/Viewer/CMakeLists.txt @@ -19,6 +19,6 @@ target_link_libraries(ViewerComp ${libraries}) set(target Viewer ViewerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/Viewer/GLscene.cpp b/rtc/Viewer/GLscene.cpp index af1dedcdb82..69174dd7170 100644 --- a/rtc/Viewer/GLscene.cpp +++ b/rtc/Viewer/GLscene.cpp @@ -7,11 +7,11 @@ #include #endif #include -#include "util/GLcamera.h" -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/LogManager.h" -#include "HRPDataTypes.hh" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/idl/HRPDataTypes.hh" #include "GLscene.h" using namespace OpenHRP; @@ -90,7 +90,7 @@ void GLscene::showStatus() int width = m_width - 410; int height = m_height-HEIGHT_STEP; char buf[256]; - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *l = glbody->joint(i); if (l){ int x = width; diff --git a/rtc/Viewer/GLscene.h b/rtc/Viewer/GLscene.h index 63aa8073ea2..e520f242fc4 100644 --- a/rtc/Viewer/GLscene.h +++ b/rtc/Viewer/GLscene.h @@ -1,7 +1,7 @@ #ifndef __GLSCENE_H__ #define __GLSCENE_H__ -#include "util/GLsceneBase.h" +#include "hrpsys/util/GLsceneBase.h" class LogManagerBase; diff --git a/rtc/Viewer/RTCGLbody.cpp b/rtc/Viewer/RTCGLbody.cpp index f041c01025c..3f23bc2e345 100644 --- a/rtc/Viewer/RTCGLbody.cpp +++ b/rtc/Viewer/RTCGLbody.cpp @@ -3,8 +3,8 @@ #if 0 #include "IrrModel.h" #else -#include "util/GLbody.h" -#include "util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" #endif RTCGLbody::RTCGLbody(GLbody *i_body, RTC::DataFlowComponentBase *comp) : diff --git a/rtc/Viewer/RTCGLbody.h b/rtc/Viewer/RTCGLbody.h index 939b9b7949f..151907d7eb3 100644 --- a/rtc/Viewer/RTCGLbody.h +++ b/rtc/Viewer/RTCGLbody.h @@ -1,6 +1,8 @@ #ifndef __RTCGLBODY_H__ #define __RTCGLBODY_H__ +#include +#include #include #include #include diff --git a/rtc/Viewer/Viewer.cpp b/rtc/Viewer/Viewer.cpp index bd9b94bee8d..bfdc3779e59 100644 --- a/rtc/Viewer/Viewer.cpp +++ b/rtc/Viewer/Viewer.cpp @@ -9,12 +9,13 @@ #include -#include "util/Project.h" +#include "hrpsys/util/Project.h" #include -#include "util/GLbody.h" -#include "util/GLlink.h" -#include "util/GLutil.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" #include "GLscene.h" +#include "hrpsys/idl/HRPDataTypes.hh" #include "RTCGLbody.h" #include "Viewer.h" diff --git a/rtc/Viewer/Viewer.h b/rtc/Viewer/Viewer.h index 19697fb7a97..640e77c0082 100644 --- a/rtc/Viewer/Viewer.h +++ b/rtc/Viewer/Viewer.h @@ -10,15 +10,16 @@ #ifndef VIEWER_H #define VIEWER_H +#include +#include "hrpsys/idl/HRPDataTypes.hh" #include #include #include #include #include #include -#include "util/LogManager.h" -#include "util/SDLUtil.h" -#include "HRPDataTypes.hh" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/util/SDLUtil.h" #include "GLscene.h" class RTCGLbody; diff --git a/rtc/VirtualCamera/CMakeLists.txt b/rtc/VirtualCamera/CMakeLists.txt index 4d6e145912b..6150c7a953a 100644 --- a/rtc/VirtualCamera/CMakeLists.txt +++ b/rtc/VirtualCamera/CMakeLists.txt @@ -16,6 +16,6 @@ target_link_libraries(VirtualCameraComp ${libraries}) set(target VirtualCamera VirtualCameraComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/VirtualCamera/GLscene.cpp b/rtc/VirtualCamera/GLscene.cpp index 967d3e76c23..37a9af4714b 100644 --- a/rtc/VirtualCamera/GLscene.cpp +++ b/rtc/VirtualCamera/GLscene.cpp @@ -7,11 +7,11 @@ #include #endif #include -#include "util/GLcamera.h" -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/LogManager.h" -#include "HRPDataTypes.hh" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/idl/HRPDataTypes.hh" #include "GLscene.h" using namespace OpenHRP; @@ -90,7 +90,7 @@ void GLscene::showStatus() int width = m_width - 410; int height = m_height-HEIGHT_STEP; char buf[256]; - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *l = glbody->joint(i); if (l){ int x = width; diff --git a/rtc/VirtualCamera/GLscene.h b/rtc/VirtualCamera/GLscene.h index 63aa8073ea2..e520f242fc4 100644 --- a/rtc/VirtualCamera/GLscene.h +++ b/rtc/VirtualCamera/GLscene.h @@ -1,7 +1,7 @@ #ifndef __GLSCENE_H__ #define __GLSCENE_H__ -#include "util/GLsceneBase.h" +#include "hrpsys/util/GLsceneBase.h" class LogManagerBase; diff --git a/rtc/VirtualCamera/RTCGLbody.cpp b/rtc/VirtualCamera/RTCGLbody.cpp index a5a424af073..a3d63fd7987 100644 --- a/rtc/VirtualCamera/RTCGLbody.cpp +++ b/rtc/VirtualCamera/RTCGLbody.cpp @@ -1,6 +1,6 @@ #include -#include "util/GLbody.h" -#include "util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" #include "RTCGLbody.h" RTCGLbody::RTCGLbody(GLbody *i_body, RTC::DataFlowComponentBase *comp) : diff --git a/rtc/VirtualCamera/RTCGLbody.h b/rtc/VirtualCamera/RTCGLbody.h index a41a0dfdca9..b8bc85fd6f2 100644 --- a/rtc/VirtualCamera/RTCGLbody.h +++ b/rtc/VirtualCamera/RTCGLbody.h @@ -1,6 +1,8 @@ #ifndef __RTCGLBODY_H__ #define __RTCGLBODY_H__ +#include +#include #include #include #include diff --git a/rtc/VirtualCamera/VirtualCamera.cpp b/rtc/VirtualCamera/VirtualCamera.cpp index 8c5f0f4d969..ccf356e174c 100644 --- a/rtc/VirtualCamera/VirtualCamera.cpp +++ b/rtc/VirtualCamera/VirtualCamera.cpp @@ -14,12 +14,12 @@ #endif #include #include -#include "util/Project.h" -#include "util/VectorConvert.h" -#include "util/GLcamera.h" -#include "util/GLbody.h" -#include "util/GLlink.h" -#include "util/GLutil.h" +#include "hrpsys/util/Project.h" +#include "hrpsys/util/VectorConvert.h" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" #include "VirtualCamera.h" #include "RTCGLbody.h" #include "GLscene.h" diff --git a/rtc/VirtualCamera/VirtualCamera.h b/rtc/VirtualCamera/VirtualCamera.h index 7932c0bbaa7..a78eeb17956 100644 --- a/rtc/VirtualCamera/VirtualCamera.h +++ b/rtc/VirtualCamera/VirtualCamera.h @@ -10,22 +10,23 @@ #ifndef VIRTUAL_CAMERA_H #define VIRTUAL_CAMERA_H +#include +#include +#include "hrpsys/idl/HRPDataTypes.hh" +#include "hrpsys/idl/Img.hh" +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include //Open CV headder #include #include // -#include "util/LogManager.h" -#include "util/SDLUtil.h" -#include "Img.hh" -#include "HRPDataTypes.hh" -#include "pointcloud.hh" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/util/SDLUtil.h" #include "GLscene.h" class GLcamera; class RTCGLbody; diff --git a/rtc/VirtualForceSensor/CMakeLists.txt b/rtc/VirtualForceSensor/CMakeLists.txt index 11e22684bf3..db5ef27bedb 100644 --- a/rtc/VirtualForceSensor/CMakeLists.txt +++ b/rtc/VirtualForceSensor/CMakeLists.txt @@ -10,6 +10,6 @@ target_link_libraries(VirtualForceSensorComp ${libs}) set(target VirtualForceSensor VirtualForceSensorComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/VirtualForceSensor/VirtualForceSensor.cpp b/rtc/VirtualForceSensor/VirtualForceSensor.cpp index 2b27ed6e9ab..92f3cdcb1a4 100644 --- a/rtc/VirtualForceSensor/VirtualForceSensor.cpp +++ b/rtc/VirtualForceSensor/VirtualForceSensor.cpp @@ -100,7 +100,7 @@ RTC::ReturnCode_t VirtualForceSensor::onInitialize() // virtual_force_sensor: , , , 0, 0, 0, 0, 0, 1, 0 coil::vstring virtual_force_sensor = coil::split(prop["virtual_force_sensor"], ","); - for(int i = 0; i < virtual_force_sensor.size()/10; i++ ){ + for(unsigned int i = 0; i < virtual_force_sensor.size()/10; i++ ){ std::string name = virtual_force_sensor[i*10+0]; VirtualForceSensorParam p; p.base_name = virtual_force_sensor[i*10+1]; @@ -196,7 +196,7 @@ RTC::ReturnCode_t VirtualForceSensor::onExecute(RTC::UniqueId ec_id) if ( m_qCurrent.data.length() == m_robot->numJoints() && m_tauIn.data.length() == m_robot->numJoints() ) { // reference model - for ( int i = 0; i < m_robot->numJoints(); i++ ){ + for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){ m_robot->joint(i)->q = m_qCurrent.data[i]; } m_robot->calcForwardKinematics(); diff --git a/rtc/VirtualForceSensor/VirtualForceSensor.h b/rtc/VirtualForceSensor/VirtualForceSensor.h index 6a7fbdd25ba..af406bee351 100644 --- a/rtc/VirtualForceSensor/VirtualForceSensor.h +++ b/rtc/VirtualForceSensor/VirtualForceSensor.h @@ -10,6 +10,7 @@ #ifndef VIRTUAL_FORCE_SENSOR_H #define VIRTUAL_FORCE_SENSOR_H +#include #include #include #include diff --git a/rtc/VirtualForceSensor/VirtualForceSensorService_impl.h b/rtc/VirtualForceSensor/VirtualForceSensorService_impl.h index b329972241a..7490d78c7b2 100644 --- a/rtc/VirtualForceSensor/VirtualForceSensorService_impl.h +++ b/rtc/VirtualForceSensor/VirtualForceSensorService_impl.h @@ -2,7 +2,7 @@ #ifndef IMPEDANCESERVICESVC_IMPL_H #define IMPEDANCESERVICESVC_IMPL_H -#include "VirtualForceSensorService.hh" +#include "hrpsys/idl/VirtualForceSensorService.hh" using namespace OpenHRP; diff --git a/rtc/VoxelGridFilter/CMakeLists.txt b/rtc/VoxelGridFilter/CMakeLists.txt index 02613050444..0c514eb28de 100644 --- a/rtc/VoxelGridFilter/CMakeLists.txt +++ b/rtc/VoxelGridFilter/CMakeLists.txt @@ -14,6 +14,6 @@ target_link_libraries(VoxelGridFilterComp ${libs}) set(target VoxelGridFilter VoxelGridFilterComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/VoxelGridFilter/VoxelGridFilter.cpp b/rtc/VoxelGridFilter/VoxelGridFilter.cpp index 0ee384e723a..fe64fb3df3f 100644 --- a/rtc/VoxelGridFilter/VoxelGridFilter.cpp +++ b/rtc/VoxelGridFilter/VoxelGridFilter.cpp @@ -11,7 +11,7 @@ #include #include #include "VoxelGridFilter.h" -#include "pointcloud.hh" +#include "hrpsys/idl/pointcloud.hh" // Module specification // @@ -29,6 +29,7 @@ static const char* spec[] = "lang_type", "compile", // Configuration variables "conf.default.size", "0.01", + "conf.default.debugLevel", "0", "" }; @@ -56,6 +57,7 @@ RTC::ReturnCode_t VoxelGridFilter::onInitialize() // // Bind variables and configuration variable bindParameter("size", m_size, "0.01"); + bindParameter("debugLevel", m_debugLevel, "0"); // @@ -136,18 +138,27 @@ RTC::ReturnCode_t VoxelGridFilter::onDeactivated(RTC::UniqueId ec_id) RTC::ReturnCode_t VoxelGridFilter::onExecute(RTC::UniqueId ec_id) { - //std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + if (m_debugLevel > 0){ + std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << ")" << std::endl; + } if (m_originalIn.isNew()){ m_originalIn.read(); + if (!m_size){ + m_filtered = m_original; + m_filteredOut.write(); + return RTC::RTC_OK; + } + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud); // RTM -> PCL + cloud->is_dense = m_original.is_dense; cloud->points.resize(m_original.width*m_original.height); float *src = (float *)m_original.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ cloud->points[i].x = src[0]; cloud->points[i].y = src[1]; cloud->points[i].z = src[2]; @@ -160,12 +171,18 @@ RTC::ReturnCode_t VoxelGridFilter::onExecute(RTC::UniqueId ec_id) sor.setLeafSize(m_size, m_size, m_size); sor.filter(*cloud_filtered); + if (m_debugLevel > 0){ + std::cout << "original:" << cloud->points.size() << ", filterd:" << cloud_filtered->points.size() << std::endl; + } + + // PCL -> RTM m_filtered.width = cloud_filtered->points.size(); + m_filtered.height = 1; m_filtered.row_step = m_filtered.point_step*m_filtered.width; m_filtered.data.length(m_filtered.height*m_filtered.row_step); float *dst = (float *)m_filtered.data.get_buffer(); - for (int i=0; ipoints.size(); i++){ + for (unsigned int i=0; ipoints.size(); i++){ dst[0] = cloud_filtered->points[i].x; dst[1] = cloud_filtered->points[i].y; dst[2] = cloud_filtered->points[i].z; diff --git a/rtc/VoxelGridFilter/VoxelGridFilter.h b/rtc/VoxelGridFilter/VoxelGridFilter.h index 29cba7e73c4..ee4379c2af5 100644 --- a/rtc/VoxelGridFilter/VoxelGridFilter.h +++ b/rtc/VoxelGridFilter/VoxelGridFilter.h @@ -10,13 +10,14 @@ #ifndef VOXEL_GRID_FILTER_H #define VOXEL_GRID_FILTER_H +#include +#include "hrpsys/idl/pointcloud.hh" #include #include #include #include #include #include -#include "pointcloud.hh" // Service implementation headers // @@ -133,6 +134,7 @@ class VoxelGridFilter // private: + int m_debugLevel; int dummy; double m_size; }; diff --git a/rtc/WavPlayer/CMakeLists.txt b/rtc/WavPlayer/CMakeLists.txt index a343b5895fa..0af5fdb96aa 100644 --- a/rtc/WavPlayer/CMakeLists.txt +++ b/rtc/WavPlayer/CMakeLists.txt @@ -10,7 +10,7 @@ target_link_libraries(WavPlayerComp ${libs}) set(target WavPlayer WavPlayerComp) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) diff --git a/rtc/WavPlayer/WavPlayer.h b/rtc/WavPlayer/WavPlayer.h index 962f307c6ab..939a4773293 100644 --- a/rtc/WavPlayer/WavPlayer.h +++ b/rtc/WavPlayer/WavPlayer.h @@ -10,6 +10,7 @@ #ifndef WAV_PLAYER_H #define WAV_PLAYER_H +#include #include #include #include diff --git a/rtc/WavPlayer/WavPlayerService_impl.cpp b/rtc/WavPlayer/WavPlayerService_impl.cpp index 881f5174969..c441297b10d 100644 --- a/rtc/WavPlayer/WavPlayerService_impl.cpp +++ b/rtc/WavPlayer/WavPlayerService_impl.cpp @@ -2,7 +2,7 @@ #include #include #include -#include "util/Hrpsys.h" +#include "hrpsys/util/Hrpsys.h" #include "WavPlayerService_impl.h" diff --git a/rtc/WavPlayer/WavPlayerService_impl.h b/rtc/WavPlayer/WavPlayerService_impl.h index ec12220cc63..3ee0ff88708 100644 --- a/rtc/WavPlayer/WavPlayerService_impl.h +++ b/rtc/WavPlayer/WavPlayerService_impl.h @@ -2,7 +2,7 @@ #ifndef WAVPLAYSERVICESVC_IMPL_H #define WAVPLAYSERVICESVC_IMPL_H -#include "WavPlayerService.hh" +#include "hrpsys/idl/WavPlayerService.hh" using namespace OpenHRP; diff --git a/sample/README.md b/sample/README.md index 10113d6a8d5..fb79e5e3b3d 100644 --- a/sample/README.md +++ b/sample/README.md @@ -1,4 +1,3 @@ -======================================================================================================================== Samples ======================================================================================================================== diff --git a/sample/Sample6dofRobot/Sample6dofRobot.xml.in b/sample/Sample6dofRobot/Sample6dofRobot.xml.in index 05d7e669c0d..41f834ae045 100644 --- a/sample/Sample6dofRobot/Sample6dofRobot.xml.in +++ b/sample/Sample6dofRobot/Sample6dofRobot.xml.in @@ -23,9 +23,10 @@ + - + diff --git a/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in index 247b8832138..bf687b60ef1 100755 --- a/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in +++ b/sample/Sample6dofRobot/sample6dofrobot_kalman_filter.py.in @@ -37,21 +37,22 @@ def test_kf_plot (time, av1, av2, av3, optional_out_file_name): # time [s] kf_ret=[] for line in open("/tmp/test-kf-sample6dofrobot-{0}s.kf_rpy".format(time), "r"): kf_ret.append(line.split(" ")[0:-1]) - seq_ret=[] - for line in open("/tmp/test-kf-sample6dofrobot-{0}s.sh_qOut".format(time,optional_out_file_name), "r"): - seq_ret.append(line.split(" ")[0:-1]) - initial_sec=int(seq_ret[0][0].split(".")[0]) - tm_list=map (lambda x : int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6, seq_ret) + # Actual rpy from simualtor : time, posx, posy, posz, roll, pitch, yaw + act_rpy_ret=[] + for line in open("/tmp/test-kf-sample6dofrobot-{0}s.Sample6dofRobot(Robot)0_WAIST".format(time), "r"): + act_rpy_ret.append(line.split(" ")[0:-1]) + initial_sec=int(act_rpy_ret[0][0].split(".")[0]) + tm_list=map (lambda x : int(x[0].split(".")[0])-initial_sec + float(x[0].split(".")[1]) * 1e-6, act_rpy_ret) plt.clf() for idx in range(3): - plt.plot(tm_list, map(lambda x : 180.0 * float(x[4+(2-idx)]) / math.pi, seq_ret)) + plt.plot(tm_list, map(lambda x : 180.0 * float(x[4+idx]) / math.pi, act_rpy_ret)) plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+idx]) / math.pi, kf_ret), ":") plt.xlabel("Time [s]") plt.ylabel("Angle [deg]") plt.title("KF actual-estimated data (motion time = {0}[s], {1})".format(time,optional_out_file_name)) plt.legend(("Actual roll", "Estimated roll", "Actual pitch", "Estimated pitch", - "Actual yaw", "Estimated yaw")) + "Actual yaw", "Estimated yaw"), loc=0) plt.savefig("/tmp/test-kf-sample6dofrobot-data-{0}s-{1}.eps".format(time,optional_out_file_name)) def demo(): @@ -74,9 +75,17 @@ def demo(): print "setKalmanFilterParam() => OK" # 3. check log and plot + offset = math.radians(85) # we need this because we rotate waist of this robot in xml file. for tm in [4, 1]: - test_kf_plot(tm, [0,0,0,0,1,0], [0,0,0,0,-1,0], [0,0,0,0,0,0], "r0-ptest") - test_kf_plot(tm, [0,0,0,0,1,0.8], [0,0,0,0,-1,0.8], [0,0,0,0,0,0.8], "r1-ptest") - test_kf_plot(tm, [0,0,0,0,0,0.8], [0,0,0,0,0,-0.8], [0,0,0,0,0,0], "p0-rtest") - test_kf_plot(tm, [0,0,0,0,1,0.8], [0,0,0,0,1,-0.8], [0,0,0,0.0,1,0], "p1-rtest") - test_kf_plot(tm, [0,0,0,0,1,0.8], [0,0,0,0,-1,-0.8], [0,0,0,0,0,0], "rptest") + test_kf_plot(tm, [0,0,0,0,1-offset,0], [0,0,0,0,-1-offset,0], [0,0,0,0,0-offset,0], "r0-ptest") + test_kf_plot(tm, [0,0,0,0,1-offset,0.8], [0,0,0,0,-1-offset,0.8], [0,0,0,0,0-offset,0.8], "r1-ptest") + test_kf_plot(tm, [0,0,0,0,0-offset,0.8], [0,0,0,0,0-offset,-0.8], [0,0,0,0,0-offset,0], "p0-rtest") + test_kf_plot(tm, [0,0,0,0,1-offset,0.8], [0,0,0,0,1-offset,-0.8], [0,0,0,0.0,1-offset,0], "p1-rtest") + test_kf_plot(tm, [0,0,0,0,1-offset,0.8], [0,0,0,0,-1-offset,-0.8], [0,0,0,0,0-offset,0], "rptest") + + # 4. check singularity + for alg in [OpenHRP.KalmanFilterService.RPYKalmanFilter, OpenHRP.KalmanFilterService.QuaternionExtendedKalmanFilter]: + kfp.kf_algorithm = alg + hcf.kf_svc.setKalmanFilterParam(kfp) + test_kf_plot(3, [0,0,0,0,math.radians(90),0], [0,0,0,0,math.radians(-90),0], [0,0,0,0,0,0], "singularity-test-pitch-"+str(alg)) + test_kf_plot(3, [0,0,0,math.radians(90),0,0], [0,0,0,math.radians(-90),0,0], [0,0,0,0,0,0], "singularity-test-yaw-"+str(alg)) diff --git a/sample/SampleRobot/CMakeLists.txt b/sample/SampleRobot/CMakeLists.txt index b236f0cead8..a9cc09800d3 100644 --- a/sample/SampleRobot/CMakeLists.txt +++ b/sample/SampleRobot/CMakeLists.txt @@ -1,6 +1,8 @@ # generate .conf and .xml files for two controller period set(controller_time_list 0.002 0.005) # [s] set(controller_period_list 500 200) # [Hz] +set(COLLISION_LOOP 1) +set(L_SHOULDER_R_INITIAL 0.0) foreach(_idx 0 1) list(GET controller_time_list ${_idx} _tmp_controller_time) list(GET controller_period_list ${_idx} _tmp_controller_period) @@ -19,6 +21,13 @@ endforeach() configure_file(SampleRobot.kinematicsonly.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.kinematicsonly.xml) configure_file(SampleRobot.carryobject.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.carryobject.xml) +## +set(CONTROLLER_TIME 0.002) +set(CONTROLLER_PERIOD 500) +set(L_SHOULDER_R_INITIAL -0.15) ## +configure_file(SampleRobot.xml.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500_init_col.xml) +set(L_SHOULDER_R_INITIAL 0.0) ## revert initial angle + # for virtual force sensor testing set(CONTROLLER_TIME 0.002) set(CONTROLLER_PERIOD 500) @@ -27,6 +36,8 @@ configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.v set(VirtualForceSensorSetting "#virtual_force_sensor") # comment out set(JointLimitTableSetting "joint_limit_table") configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.el.conf) +set(COLLISION_LOOP 3) +configure_file(SampleRobot.conf.in ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.co_loop.conf) set(robot_waist_translation "-0.8 0 0" "-4.15 -0.2 0" "-5.83 -0.2 -0.6096") set(file_suffix "SlopeUpDown" "StairUp" "StairDown") @@ -42,6 +53,7 @@ install(FILES ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.conf ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.log.conf ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.xml + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500_init_col.xml ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.torque.xml ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.RobotHardware.500.conf # files for 200[Hz] = 0.005[s] @@ -55,6 +67,8 @@ install(FILES # for SoftErrorLimiter ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.el.conf SampleRobot.PDgain.dat + # for collision_loop + ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.500.co_loop.conf # TerrainFloor ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.SlopeUpDown.xml ${CMAKE_CURRENT_BINARY_DIR}/SampleRobot.TerrainFloor.StairUp.xml diff --git a/sample/SampleRobot/README.md b/sample/SampleRobot/README.md index b4d74e3fafa..5ac7fe7b00f 100644 --- a/sample/SampleRobot/README.md +++ b/sample/SampleRobot/README.md @@ -1,4 +1,3 @@ -======================================================================================================================== Examples ======================================================================================================================== @@ -10,297 +9,335 @@ To learn more about API, please refer to [API DOC in hrpsys-base](http://fkanehi # samplerobot-walk.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot-walk.py - ``` - This example imports [hrpsys-base samplerobot_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_walk.py) for mor info. + ``` + rosrun hrpsys samplerobot-walk.py + ``` + + This example imports [hrpsys-base samplerobot_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_walk.py) for mor info. # samplerobot-data-logger.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot_data_logger.py - ``` - See - [hrpsys-base samplerobot_data_logger.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_data_logger.py) for more info. + ``` + rosrun hrpsys samplerobot_data_logger.py + ``` + + See [hrpsys-base samplerobot_data_logger.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_data_logger.py) for more info. 3. RTC explanation - - DataLogger - DataLogger is hrpsys-base RTC for data logging of hrpsys-base RTCs. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d4/d46/DataLogger.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/d7/dbb/interfaceOpenHRP_1_1DataLoggerService.html). - - Save log - By using ``save()`` function, DataLogger saves data ports values in a ring buffer into several files. If data ports are connected by ``connectLoggerPort`` in [hrpsys-base hrpsys_config.py](https://github.com/fkanehiro/hrpsys-base/blob/master/python/hrpsys_config.py), file extensions are determined by RTC names and data port names. - In the above example, files are saved as ``/tmp/test-samplerobot-log.**``. - The log file for StateHolder RTC's ``qOut`` data port is written as ``/tmp/test-samplerobot-log.sh_qOut``. - ``sh`` is the comonent name for StateHolder, which is specified in - [hrpsys-base hrpsys_config.py's getRTCList function](https://github.com/fkanehiro/hrpsys-base/blob/master/python/hrpsys_config.py). + - DataLogger + DataLogger is hrpsys-base RTC for data logging of hrpsys-base RTCs. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d4/d46/DataLogger.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/d7/dbb/interfaceOpenHRP_1_1DataLoggerService.html). + - Save log + By using ``save()`` function, DataLogger saves data ports values in a ring buffer into several files. If data ports are connected by ``connectLoggerPort`` in [hrpsys-base hrpsys_config.py](https://github.com/fkanehiro/hrpsys-base/blob/master/python/hrpsys_config.py), file extensions are determined by RTC names and data port names. + In the above example, files are saved as ``/tmp/test-samplerobot-log.**``. + The log file for StateHolder RTC's ``qOut`` data port is written as ``/tmp/test-samplerobot-log.sh_qOut``. + ``sh`` is the comonent name for StateHolder, which is specified in + [hrpsys-base hrpsys_config.py's getRTCList function](https://github.com/fkanehiro/hrpsys-base/blob/master/python/hrpsys_config.py). # samplerobot_remove_force_offset.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` -2. python example - - ``` -rosrun hrpsys samplerobot_remove_force_offset.py - ``` - See - [hrpsys-base samplerobot_remove_force_offset.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_remove_force_offset.py) for more info. -3. RTC explanation - - RemoveForceSensorLinkOffset - RemoveForceSensorLinkOffset is hrpsys-base RTC to remove hands or feet from force sensor values. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/dc/d76/RemoveForceSensorLinkOffset.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/d6/d02/interfaceOpenHRP_1_1RemoveForceSensorLinkOffsetService.html). - - Offsetting - In the above example, the initial wrench values are larger than zero: - ``` - # 1. force and moment are large because of link offsets - fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data) - print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm > 1e-2 - fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) - print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm > 1e-2 + rtmlaunch hrpsys samplerobot.launch ``` - This is because these values include hand link weight force and weight moment. - After offsetting by ``setForceMomentOffsetParam()`` function, - wrench values become almost zero: +2. python example ``` - # 3. force and moment are reduced - fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data) - print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm < 1e-2 - fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) - print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm < 1e-2 + rosrun hrpsys samplerobot_remove_force_offset.py ``` + See [hrpsys-base samplerobot_remove_force_offset.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_remove_force_offset.py) for more info. +3. RTC explanation + - RemoveForceSensorLinkOffset + RemoveForceSensorLinkOffset is hrpsys-base RTC to remove hands or feet from force sensor values. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/dc/d76/RemoveForceSensorLinkOffset.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/d6/d02/interfaceOpenHRP_1_1RemoveForceSensorLinkOffsetService.html). + - Offsetting + In the above example, the initial wrench values are larger than zero: + + ``` + # 1. force and moment are large because of link offsets + fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data) + print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm > 1e-2 + fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) + print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm > 1e-2 + ``` + + This is because these values include hand link weight force and weight moment. + After offsetting by ``setForceMomentOffsetParam()`` function, + wrench values become almost zero: + + ``` + # 3. force and moment are reduced + fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data) + print "no-offset-removed force moment (rhsensor) ", fm, "=> ", fm < 1e-2 + fm=numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data) + print "no-offset-removed force moment (lhsensor) ", fm, "=> ", fm < 1e-2 + ``` + # samplerobot_impedance_controller.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot_impedance_controller.py - ``` - See - [hrpsys-base samplerobot_impedance_controller.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_impedance_controller.py) for more info. + ``` + rosrun hrpsys samplerobot_impedance_controller.py + ``` + + See [hrpsys-base samplerobot_impedance_controller.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_impedance_controller.py) for more info. 3. RTC explanation - - ImpedanceController - ImpedanceController is hrpsys-base RTC for cartesian impedance control. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d2/d9f/ImpedanceController.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/d9/d8b/interfaceOpenHRP_1_1ImpedanceControllerService.html). + - ImpedanceController + ImpedanceController is hrpsys-base RTC for cartesian impedance control. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d2/d9f/ImpedanceController.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/d9/d8b/interfaceOpenHRP_1_1ImpedanceControllerService.html). # samplerobot_emergency_stopper.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot_emergency_stopper.py - ``` - See - [hrpsys-base samplerobot_emergency_stopper.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_emergency_stopper.py) for more info. + ``` + rosrun hrpsys samplerobot_emergency_stopper.py + ``` + + See [hrpsys-base samplerobot_emergency_stopper.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_emergency_stopper.py) for more info. 3. RTC explanation - - EmergencyStopper - EmergencyStopper is hrpsys-base RTC to stop robot's motion emergently. + - EmergencyStopper + EmergencyStopper is hrpsys-base RTC to stop robot's motion emergently. # samplerobot_collision_detector.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot_collision_detector.py - ``` - See - [hrpsys-base samplerobot_collision_detector.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_collision_detector.py) for more info. + ``` + rosrun hrpsys samplerobot_collision_detector.py + ``` + See [hrpsys-base samplerobot_collision_detector.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_collision_detector.py) for more info. 3. RTC explanation - - CollisionDetector - CollisionDetector is hrpsys-base RTC to stop robot's motion when self-collision occurs and avoid from self-collision. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d7/de4/CollisionDetector.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/da/d18/interfaceOpenHRP_1_1CollisionDetectorService.html). + - CollisionDetector + CollisionDetector is hrpsys-base RTC to stop robot's motion when self-collision occurs and avoid from self-collision. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d7/de4/CollisionDetector.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/da/d18/interfaceOpenHRP_1_1CollisionDetectorService.html). # samplerobot_kalman_filter.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot_kalman_filter.py - ``` - See - [hrpsys-base samplerobot_kalman_filter.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_kalman_filter.py) for more info. + ``` + rosrun hrpsys samplerobot_kalman_filter.py + ``` + + See [hrpsys-base samplerobot_kalman_filter.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_kalman_filter.py) for more info. 3. RTC explanation - - KalmanFilter - KalmanFilter is hrpsys-base RTC to estimate robot's attitude from gyro and acceleration sensor values. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d3/de6/KalmanFilter.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/dd/d2d/interfaceOpenHRP_1_1KalmanFilterService.html). + - KalmanFilter + KalmanFilter is hrpsys-base RTC to estimate robot's attitude from gyro and acceleration sensor values. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d3/de6/KalmanFilter.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/dd/d2d/interfaceOpenHRP_1_1KalmanFilterService.html). # samplerobot_soft_error_limiter.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch CONF_FILE:=`rospack find hrpsys`/samples/SampleRobot/SampleRobot.500.el.conf - ``` + ``` + rtmlaunch hrpsys samplerobot.launch CONF_FILE:=`rospack find hrpsys`/samples/SampleRobot/SampleRobot.500.el.conf + ``` 2. python example - ``` -rosrun hrpsys samplerobot_soft_error_limiter.py - ``` - See - [hrpsys-base samplerobot_soft_error_limiter.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_soft_error_limiter.py) for more info. + ``` + rosrun hrpsys samplerobot_soft_error_limiter.py + ``` + + See [hrpsys-base samplerobot_soft_error_limiter.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_soft_error_limiter.py) for more info. 3. RTC explanation - - SoftErrorLimiter - SoftErrorLimiter is hrpsys-base RTC to perform position, error, and velocity limitation. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d9/de2/SoftErrorLimiter.htmlhttp://fkanehiro.github.io/hrpsys-base/de/d6d/interfaceOpenHRP_1_1SoftErrorLimit) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/de/d6d/interfaceOpenHRP_1_1SoftErrorLimiterService.html). + - SoftErrorLimiter + SoftErrorLimiter is hrpsys-base RTC to perform position, error, and velocity limitation. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d9/de2/SoftErrorLimiter.htmlhttp://fkanehiro.github.io/hrpsys-base/de/d6d/interfaceOpenHRP_1_1SoftErrorLimit) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/de/d6d/interfaceOpenHRP_1_1SoftErrorLimiterService.html). # samplerobot_auto_balancer.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch - ``` + ``` + rtmlaunch hrpsys samplerobot.launch + ``` 2. python example - ``` -rosrun hrpsys samplerobot_auto_balancer.py - ``` -

    AutoBalancer

    - See - [hrpsys-base samplerobot_auto_balancer.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_auto_balancer.py) for more info. + ``` + rosrun hrpsys samplerobot_auto_balancer.py + ``` + +

    AutoBalancer

    + + See [hrpsys-base samplerobot_auto_balancer.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_auto_balancer.py) for more info. + 3. RTC explanation - - AutoBalancer - AutoBalancer is hrpsys-base RTC to generate walking pattern and control Center Of Gravity for legged robots. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d1/d15/AutoBalancer.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/d4/d5b/interfaceOpenHRP_1_1AutoBalancerService.html). + - AutoBalancer + AutoBalancer is hrpsys-base RTC to generate walking pattern and control Center Of Gravity for legged robots. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d1/d15/AutoBalancer.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/d4/d5b/interfaceOpenHRP_1_1AutoBalancerService.html). # samplerobot_stabilizer.py 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch TORQUE_CONTROL:=true - ``` + ``` + rtmlaunch hrpsys samplerobot.launch TORQUE_CONTROL:=true + ``` 2. python example - ``` -rosrun hrpsys samplerobot_stabilizer.py - ``` - See - [hrpsys-base samplerobot_stabilizer.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_stabilizer.py) for more info. + ``` + rosrun hrpsys samplerobot_stabilizer.py + ``` + + See [hrpsys-base samplerobot_stabilizer.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_stabilizer.py) for more info. 3. RTC explanation - - Stabilizer - Stabilizer is hrpsys-base RTC to maintain full-body balance based on sensor feedback. - Please see - [Overview](http://fkanehiro.github.io/hrpsys-base/d6/d76/Stabilizer.html) - and [IDL API](http://fkanehiro.github.io/hrpsys-base/d5/dc8/interfaceOpenHRP_1_1StabilizerService.html). + - Stabilizer + Stabilizer is hrpsys-base RTC to maintain full-body balance based on sensor feedback. + Please see + [Overview](http://fkanehiro.github.io/hrpsys-base/d6/d76/Stabilizer.html) + and [IDL API](http://fkanehiro.github.io/hrpsys-base/d5/dc8/interfaceOpenHRP_1_1StabilizerService.html). # samplerobot_carry_object.py +

    +DualarmCarry +SinglearmCarry +DualarmPush +

    + 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot.launch TORQUE_CONTROL:=true PROJECT_FILE:=`rospack find hrpsys`/share/hrpsys/samples/SampleRobot/SampleRobot.carryobject.xml - ``` + ``` + rtmlaunch hrpsys samplerobot.launch TORQUE_CONTROL:=true PROJECT_FILE:=`rospack find hrpsys`/share/hrpsys/samples/SampleRobot/SampleRobot.carryobject.xml + ``` 2. python example - ``` -rosrun hrpsys samplerobot_carry_object.py - ``` - See - [hrpsys-base samplerobot_carry_object.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_carry_object.py) for more info. + ``` + rosrun hrpsys samplerobot_carry_object.py + ``` + + See [hrpsys-base samplerobot_carry_object.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_carry_object.py) for more info. 3. Sample explanation This sample shows dual-arm lift-up, single-arm lift-up, and pushing manipulation. # samplerobot_terrain_walk.py 0. These examples are related with AutoBalancer RTC. 1. Example for slope walking -

    Slope

    - 1-1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot-terrain-walk.launch MODEL:=SlopeUpDown - ``` - 1-2. python example - ``` -rosrun hrpsys samplerobot_terrain_walk.py --SlopeUpDown - ``` - This Example imports - [hrpsys-base samplerobot_terrain_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_terrain_walk.py). + +

    Slope

    + + 1-1. Launch hrpsys-simulator + ``` + rtmlaunch hrpsys samplerobot-terrain-walk.launch MODEL:=SlopeUpDown + ``` + 1-2. python example + ``` + rosrun hrpsys samplerobot_terrain_walk.py --SlopeUpDown + ``` + + This Example imports [hrpsys-base samplerobot_terrain_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_terrain_walk.py). 2. Example for stair climbing-up -

    StairUp

    - 2-1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot-terrain-walk.launch MODEL:=StairUp - ``` - 2-2. python example - ``` -rosrun hrpsys samplerobot-terrain-walk.py --StairUp - ``` - This example imports - [hrpsys-base samplerobot_terrain_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_terrain_walk.py). + +

    StairUp

    + + 2-1. Launch hrpsys-simulator + ``` + rtmlaunch hrpsys samplerobot-terrain-walk.launch MODEL:=StairUp + ``` + 2-2. python example + ``` + rosrun hrpsys samplerobot-terrain-walk.py --StairUp + ``` + + This example imports [hrpsys-base samplerobot_terrain_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_terrain_walk.py). 3. Example for stair climbing-down -

    StairDown

    - 3-1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot-terrain-walk.launch MODEL:=StairDown - ``` - 2-2. python example - ``` -rosrun hrpsys samplerobot-terrain-walk.py --StairDown - ``` - This example imports - [hrpsys-base samplerobot_terrain_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_terrain_walk.py). + +

    StairDown

    + + 3-1. Launch hrpsys-simulator + ``` + rtmlaunch hrpsys samplerobot-terrain-walk.launch MODEL:=StairDown + ``` + 3-2. python example + ``` + rosrun hrpsys samplerobot-terrain-walk.py --StairDown + ``` + + This example imports [hrpsys-base samplerobot_terrain_walk.py](https://github.com/fkanehiro/hrpsys-base/blob/master/sample/SampleRobot/samplerobot_terrain_walk.py). # samplerobot-drc-testbed.launch 1. Launch hrpsys-simulator - ``` -rtmlaunch hrpsys samplerobot-drc-testbed.launch - ``` + ``` + rtmlaunch hrpsys samplerobot-drc-testbed.launch + ``` 2. python example - ``` -ipython -i `rospack find hrpsys`/samples/SampleRobot/samplerobot_terrain_walk.py - ``` + ``` + ipython -i `rospack find hrpsys`/samples/SampleRobot/samplerobot_terrain_walk.py + ``` 3. Programming -

    DRC Testbed

    - - Python Interface - From python iterface, we can use `hcf` as interface object to the robot, for example -`hcf.setJointAngle('LARM_SHOULDER_P', -90, 1)` - moves shoulder pitch joint of left arm to 90 degree, with 1 seconds. - Please see [Pyton API](http://fkanehiro.github.io/hrpsys-base/df/d98/classpython_1_1hrpsys__config_1_1HrpsysConfigurator.html) for the avilable method functions, and [SampleRobot Model](http://www.openrtp.jp/openhrp3/en/sample_model.html) for the list of joint names. + +

    DRC Testbed

    + + - Python Interface + From python iterface, we can use `hcf` as interface object to the robot, for example + `hcf.setJointAngle('LARM_SHOULDER_P', -90, 1)` + moves shoulder pitch joint of left arm to 90 degree, with 1 seconds. + Please see [Pyton API](http://fkanehiro.github.io/hrpsys-base/df/d98/classpython_1_1hrpsys__config_1_1HrpsysConfigurator.html) for the avilable method functions, and [SampleRobot Model](http://www.openrtp.jp/openhrp3/en/sample_model.html) for the list of joint names. + +# How to generate SampleRobot*.xml.in + Go to hrpsys source (top) directory. +1. SampleRobot.torque.xml.in + ``` + openhrp-project-generator `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/sample1_bush.wrl `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/floor5.wrl `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/box.wrl,0.78,0,0,1,0,0,0 --use-highgain-mode false --output /tmp/SampleRobot.xml --timeStep 0.001 --dt "@CONTROLLER_TIME@" --method RUNGE_KUTTA + TMP=$(rospack find openhrp3); sed -i -e "s/${TMP//\//\\/}/@OPENHRP\_DIR@/g" /tmp/SampleRobot.xml + yes | cp /tmp/SampleRobot.xml sample/SampleRobot/SampleRobot.torque.xml.in + ``` +2. SampleRobot.xml.in + ``` + openhrp-project-generator `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/sample1.wrl `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/floor5.wrl --output /tmp/SampleRobot.xml --timeStep "@CONTROLLER_TIME@" --dt "@CONTROLLER_TIME@" --method EULER --joint-properties "LARM_SHOULDER_R.angle,@L_SHOULDER_R_INITIAL@" + TMP=$(rospack find openhrp3); sed -i -e "s/${TMP//\//\\/}/@OPENHRP\_DIR@/g" /tmp/SampleRobot.xml + yes | cp /tmp/SampleRobot.xml sample/SampleRobot/SampleRobot.xml.in + ``` +3. SampleRobot.kinematicsonly.xml.in + ``` + openhrp-project-generator `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/sample1.wrl `rospack find openhrp3`/share/OpenHRP-3.1/sample/model/floor5.wrl --output /tmp/SampleRobot.xml --timeStep 0.002 --dt 0.002 --method EULER --integrate false + TMP=$(rospack find openhrp3); sed -i -e "s/${TMP//\//\\/}/@OPENHRP\_DIR@/g" /tmp/SampleRobot.xml + yes | cp /tmp/SampleRobot.xml sample/SampleRobot/SampleRobot.kinematicsonly.xml.in + ``` diff --git a/sample/SampleRobot/SampleRobot.DRCTestbed.xml b/sample/SampleRobot/SampleRobot.DRCTestbed.xml index 726e6479fa1..92e0e044ad0 100644 --- a/sample/SampleRobot/SampleRobot.DRCTestbed.xml +++ b/sample/SampleRobot/SampleRobot.DRCTestbed.xml @@ -268,6 +268,17 @@ + + + + + + + + + + + @@ -408,5 +419,16 @@ + + + + + + + + + + + diff --git a/sample/SampleRobot/SampleRobot.conf.in b/sample/SampleRobot/SampleRobot.conf.in index 93754afe15b..d384f28fa25 100644 --- a/sample/SampleRobot/SampleRobot.conf.in +++ b/sample/SampleRobot/SampleRobot.conf.in @@ -5,6 +5,7 @@ abc_leg_offset: 0,0.09,0 end_effectors: lleg,LLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, rleg,RLEG_ANKLE_R,WAIST,0.0,0.0,-0.07,0.0,0.0,0.0,0.0, larm,LARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708, rarm,RARM_WRIST_P,CHEST,0.0,0,-0.12,0,1.0,0.0,1.5708, # CollisionDetector Setting +collision_loop: @COLLISION_LOOP@ collision_pair: RARM_WRIST_P:WAIST LARM_WRIST_P:WAIST RARM_WRIST_P:RLEG_HIP_R LARM_WRIST_P:LLEG_HIP_R RARM_WRIST_R:RLEG_HIP_R LARM_WRIST_R:LLEG_HIP_R LLEG_ANKLE_R:RLEG_ANKLE_R # Collision mask not to control legs joints #collision_mask: 0,0,0,0,0,0,1,1,1,1,1,1,1,0,0,0,0,0,0,1,1,1,1,1,1,1,1,1,1 diff --git a/sample/SampleRobot/SampleRobot.kinematicsonly.xml.in b/sample/SampleRobot/SampleRobot.kinematicsonly.xml.in index 83e82a06944..edf79d1ccb0 100644 --- a/sample/SampleRobot/SampleRobot.kinematicsonly.xml.in +++ b/sample/SampleRobot/SampleRobot.kinematicsonly.xml.in @@ -21,14 +21,15 @@ + + - @@ -141,22 +142,21 @@ - - + + - - - - - + + + + @@ -167,7 +167,7 @@ - + @@ -180,13 +180,13 @@ - + - + diff --git a/sample/SampleRobot/SampleRobot.torque.xml.in b/sample/SampleRobot/SampleRobot.torque.xml.in index 15f052967ba..807161611d4 100644 --- a/sample/SampleRobot/SampleRobot.torque.xml.in +++ b/sample/SampleRobot/SampleRobot.torque.xml.in @@ -13,18 +13,19 @@ - + + + - @@ -137,21 +138,18 @@ - - - - - - - + + + + - - - - - + + + + + @@ -162,7 +160,42 @@ - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -175,14 +208,36 @@ - + + + + + + + + + + + + + + + + + + + + + + + - - + + diff --git a/sample/SampleRobot/SampleRobot.xml.in b/sample/SampleRobot/SampleRobot.xml.in index ce6cbbe7933..cf4f192cc6e 100644 --- a/sample/SampleRobot/SampleRobot.xml.in +++ b/sample/SampleRobot/SampleRobot.xml.in @@ -20,14 +20,15 @@ + + - @@ -93,7 +94,7 @@ - + @@ -140,21 +141,20 @@ - - + + - - - - - + + + + @@ -165,7 +165,7 @@ - + @@ -178,13 +178,13 @@ - + - + diff --git a/sample/SampleRobot/img/ABC.png b/sample/SampleRobot/img/ABC.png new file mode 100644 index 00000000000..069da404705 Binary files /dev/null and b/sample/SampleRobot/img/ABC.png differ diff --git a/sample/SampleRobot/img/DRCTestbed.png b/sample/SampleRobot/img/DRCTestbed.png new file mode 100644 index 00000000000..55e674e8f62 Binary files /dev/null and b/sample/SampleRobot/img/DRCTestbed.png differ diff --git a/sample/SampleRobot/img/DualarmCarry.png b/sample/SampleRobot/img/DualarmCarry.png new file mode 100644 index 00000000000..41eb1c5a8d0 Binary files /dev/null and b/sample/SampleRobot/img/DualarmCarry.png differ diff --git a/sample/SampleRobot/img/DualarmPush.png b/sample/SampleRobot/img/DualarmPush.png new file mode 100644 index 00000000000..96d2147e3b8 Binary files /dev/null and b/sample/SampleRobot/img/DualarmPush.png differ diff --git a/sample/SampleRobot/img/SinglearmCarry.png b/sample/SampleRobot/img/SinglearmCarry.png new file mode 100644 index 00000000000..20e4f3579c3 Binary files /dev/null and b/sample/SampleRobot/img/SinglearmCarry.png differ diff --git a/sample/SampleRobot/img/Slope.png b/sample/SampleRobot/img/Slope.png new file mode 100644 index 00000000000..459586609a8 Binary files /dev/null and b/sample/SampleRobot/img/Slope.png differ diff --git a/sample/SampleRobot/img/StairDown.png b/sample/SampleRobot/img/StairDown.png new file mode 100644 index 00000000000..1cf7223b662 Binary files /dev/null and b/sample/SampleRobot/img/StairDown.png differ diff --git a/sample/SampleRobot/img/StairUp.png b/sample/SampleRobot/img/StairUp.png new file mode 100644 index 00000000000..4390f08a18f Binary files /dev/null and b/sample/SampleRobot/img/StairUp.png differ diff --git a/sample/SampleRobot/rtc/CMakeLists.txt b/sample/SampleRobot/rtc/CMakeLists.txt index 8042d00fce6..555417c8e0d 100644 --- a/sample/SampleRobot/rtc/CMakeLists.txt +++ b/sample/SampleRobot/rtc/CMakeLists.txt @@ -52,8 +52,8 @@ install(PROGRAMS ) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug - LIBRARY DESTINATION lib CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin + LIBRARY DESTINATION lib ) install(CODE " execute_process(COMMAND cmake -E create_symlink ../../../lib/SampleComponent.so SampleComponent.so WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/share/hrpsys/lib) diff --git a/sample/SampleRobot/rtc/SampleComponent.cpp b/sample/SampleRobot/rtc/SampleComponent.cpp index 5deabd47804..40513b6cbee 100644 --- a/sample/SampleRobot/rtc/SampleComponent.cpp +++ b/sample/SampleRobot/rtc/SampleComponent.cpp @@ -133,7 +133,7 @@ RTC::ReturnCode_t SampleComponent::onExecute(RTC::UniqueId ec_id) if ( m_qCurrent.data.length() != m_q.data.length() ) { m_q.data.length(m_qCurrent.data.length()); } - for ( int i = 0; i < m_qCurrent.data.length(); i++ ){ + for ( unsigned int i = 0; i < m_qCurrent.data.length(); i++ ){ m_q.data[i] = m_qCurrent.data[i]; } if ( loop < 1000 ) { diff --git a/sample/SampleRobot/rtc/SampleComponent.h b/sample/SampleRobot/rtc/SampleComponent.h index e21b53a5177..d37fc89cdc7 100644 --- a/sample/SampleRobot/rtc/SampleComponent.h +++ b/sample/SampleRobot/rtc/SampleComponent.h @@ -10,6 +10,7 @@ #ifndef NULL_COMPONENT_H #define NULL_COMPONENT_H +#include #include #include #include diff --git a/sample/SampleRobot/samplerobot_auto_balancer.py b/sample/SampleRobot/samplerobot_auto_balancer.py index 24dcaf38bfd..606ded99173 100755 --- a/sample/SampleRobot/samplerobot_auto_balancer.py +++ b/sample/SampleRobot/samplerobot_auto_balancer.py @@ -96,7 +96,7 @@ def checkGoPosParam (goalx, goaly, goalth, prev_dst_foot_midcoords): ''' # Check diff [difx, dify, difth] = calcDiffFootMidCoords(prev_dst_foot_midcoords) - ret = (abs(difx-goalx) < 1e-5 and abs(dify-goaly) < 1e-5 and abs(difth-goalth) < 1e-5) + ret = (abs(difx-goalx) < 5e-5 and abs(dify-goaly) < 5e-5 and abs(difth-goalth) < 1e-2) print >> sys.stderr, " Check goPosParam (diff = ", (difx-goalx), "[m], ", (dify-goaly), "[m], ", (difth-goalth), "[deg])" print >> sys.stderr, " => ", ret assert(ret) @@ -238,6 +238,63 @@ def demoAutoBalancerBalanceWithArms(): hcf.seq_svc.setJointAngles(initial_pose, 1.0) hcf.waitInterpolation() +def demoGaitGeneratorBaseTformCheck (): + print >> sys.stderr, "0. baseTform check" + # Set parameter + orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + orig_abcp = hcf.abc_svc.getAutoBalancerParam()[1] + hcf.co_svc.disableCollisionDetection() + ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + ggp.stride_parameter = [0.2, 0.1, 20, 0.15] + ggp.default_step_time = 0.5 + hcf.abc_svc.setGaitGeneratorParam(ggp) + abcp = hcf.abc_svc.getAutoBalancerParam()[1] + abcp.transition_time = 0.1 + hcf.abc_svc.setAutoBalancerParam(abcp) + btf0 = checkParameterFromLog("baseTformOut", rtc_name="abc") + # Check start ABC + hcf.seq_svc.setJointAnglesSequenceFull([[0.000242, -0.403476, -0.000185, 0.832071, -0.427767, -6.928952e-05, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, 0.000242, -0.403469, -0.000185, 0.832073, -0.427775, -6.928781e-05, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # jvss + [[0]*29], # vels + [[0]*29], # torques + [[btf0[0]+0.2+-0.014759, btf0[1]+-0.1+-4.336272e-05, 0.668138]], # poss + [[-0.000245, -0.000862, 0.000171]], # rpys + [[0]*3], # accs + [[0.014052, 0.000203, -0.66798]], # zmps + [[0]*6*4], # wrenchs + [[1,1,0,0,1,1,1,1]], # optionals + [0.5]); # tms + hcf.waitInterpolation() + btf1 = checkParameterFromLog("baseTformOut", rtc_name="abc") + hcf.startAutoBalancer() + btf2 = checkParameterFromLog("baseTformOut", rtc_name="abc") + # Check stop ABC + hcf.abc_svc.goPos(-0.2, 0.1, 0) + hcf.abc_svc.waitFootSteps() + btf3 = checkParameterFromLog("baseTformOut", rtc_name="abc") + hcf.seq_svc.setJointAnglesSequenceFull([[0.000242, -0.403476, -0.000185, 0.832071, -0.427767, -6.928952e-05, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, 0.000242, -0.403469, -0.000185, 0.832073, -0.427775, -6.928781e-05, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # jvss + [[0]*29], # vels + [[0]*29], # torques + [[btf0[0]+-0.014759, btf0[1]+-4.336272e-05, 0.668138]], # poss + [[-0.000245, -0.000862, 0.000171]], # rpys + [[0]*3], # accs + [[0.014052, 0.000203, -0.66798]], # zmps + [[0]*6*4], # wrenchs + [[1,1,0,0,1,1,1,1]], # optionals + [0.1]); # tms + hcf.waitInterpolation() + hcf.stopAutoBalancer() + btf4 = checkParameterFromLog("baseTformOut", rtc_name="abc") + # Finalize + hcf.abc_svc.setGaitGeneratorParam(orig_ggp) + hcf.abc_svc.setAutoBalancerParam(orig_abcp) + hcf.co_svc.enableCollisionDetection() + # Check values (currently pos x,y only 1[mm]) + startABC_OK = all(map (lambda x,y : abs(x-y)<1*1e-3, btf1[0:3], btf2[0:3])) + stopABC_OK = all(map (lambda x,y : abs(x-y)<1*1e-3, btf3[0:3], btf4[0:3])) + print >> sys.stderr, " before startABC = ", btf1[0:3], ", after startABC = ", btf2[0:3], ", diff = ", startABC_OK + print >> sys.stderr, " before stopABC = ", btf3[0:3], ", after stopABC = ", btf4[0:3], ", diff = ", stopABC_OK + assert(startABC_OK and stopABC_OK) + def demoGaitGeneratorGoPos(): print >> sys.stderr, "1. goPos" hcf.startAutoBalancer(); @@ -557,6 +614,9 @@ def demoGaitGeneratorGoPosOverwrite(): print >> sys.stderr, "16. goPos overwriting" hcf.startAutoBalancer(); print >> sys.stderr, " Overwrite goPos by goPos" + # Initialize dst_foot_midcoords + hcf.abc_svc.goPos(0,0.001,0); + hcf.abc_svc.waitFootSteps(); goalx=0.3;goaly=0.1;goalth=15.0 prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords hcf.abc_svc.goPos(0.2,-0.1,-5) # initial gopos @@ -610,16 +670,17 @@ def demoGaitGeneratorGrasplessManipMode(): gv_pose_list = [av_fwd, av_bwd, av_left, av_right, av_lturn, av_rturn] ref_footmid_diff = [[50*1e-3,0,0], [-50*1e-3,0,0], - [0, 0.5*50*1e-3,0], # 0.5->inside limitation - [0,-0.5*50*1e-3,0], # 0.5->inside limitation - [0,0, 0.5*10], # 0.5->inside limitation - [0,0,-0.5*10]] # 0.5->inside limitation + [0, 50*1e-3,0], + [0,-50*1e-3,0], + [0,0, 10], + [0,0,-10]] ret=True hcf.abc_svc.waitFootSteps() for idx in range(len(gv_pose_list)): pose = gv_pose_list[idx] prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords - hcf.abc_svc.goVelocity(0,0,0); + yvel = -0.0001 if (idx%2==0) else 0.0001 # Iff even number test, start with rleg. Otherwise, lleg. + hcf.abc_svc.goVelocity(0,yvel,0); hcf.seq_svc.setJointAngles(pose, 0.4) hcf.waitInterpolation() hcf.seq_svc.setJointAngles(pose, 1.6);hcf.waitInterpolation() # Dummy 2step @@ -671,6 +732,36 @@ def demoGaitGeneratorSetFootStepsWithArms(): hcf.abc_svc.setGaitGeneratorParam(orig_ggp) hcf.startAutoBalancer() +def demoGaitGeneratorChangeStrideLimitationType(): + print >> sys.stderr, "19. Change stride limitation type to CIRCLE" + hcf.startAutoBalancer(); + # initialize dst_foot_midcoords + hcf.abc_svc.goPos(0,0,0) + hcf.abc_svc.waitFootSteps() + # set params + orig_ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + ggp.stride_limitation_type = OpenHRP.AutoBalancerService.CIRCLE + ggp.stride_limitation_for_circle_type = [0.15, 0.25, 10, 0.1, 0.1] + ggp.leg_margin = [182.0*1e-3, 72.0*1e-3, 71.12*1e-3, 71.12*1e-3] + hcf.abc_svc.setGaitGeneratorParam(ggp) + # gopos check 1 + goalx=0.3;goaly=0.3;goalth=20.0 + prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords + hcf.abc_svc.goPos(goalx, goaly, goalth) + hcf.abc_svc.waitFootSteps() + checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords) + # gopos check 2 + goalx=-0.3;goaly=-0.3;goalth=-10.0 + prev_dst_foot_midcoords=hcf.abc_svc.getFootstepParam()[1].dst_foot_midcoords + hcf.abc_svc.goPos(goalx, goaly, goalth) + hcf.abc_svc.waitFootSteps() + checkGoPosParam(goalx, goaly, goalth, prev_dst_foot_midcoords) + checkActualBaseAttitude() + print >> sys.stderr, " Change stride limitation type to CIRCLE=>OK" + # reset params + hcf.abc_svc.setGaitGeneratorParam(orig_ggp) + def demoStandingPosResetting(): print >> sys.stderr, "demoStandingPosResetting" hcf.abc_svc.goPos(0,0,math.degrees(-1*checkParameterFromLog("WAIST")[5])); # Rot yaw @@ -679,8 +770,10 @@ def demoStandingPosResetting(): hcf.abc_svc.waitFootSteps() def demo(): + start_time = time.time() init() - if hrpsys_version >= '315.5.0': + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): # sample for AutoBalancer mode demoAutoBalancerFixFeet() demoAutoBalancerFixFeetHands() @@ -691,6 +784,7 @@ def demo(): demoAutoBalancerBalanceAgainstHandForce() demoAutoBalancerBalanceWithArms() # sample for walk pattern generation by AutoBalancer RTC + demoGaitGeneratorBaseTformCheck() demoGaitGeneratorGoPos() demoGaitGeneratorGoVelocity() demoGaitGeneratorSetFootSteps() @@ -705,12 +799,14 @@ def demo(): demoGaitGeneratorChangeStepParam() demoGaitGeneratorOverwriteFootsteps() demoGaitGeneratorOverwriteFootsteps(2) - demoStandingPosResetting() + #demoStandingPosResetting() demoGaitGeneratorFixHand() demoGaitGeneratorOverwriteCurrentFootstep() demoGaitGeneratorGoPosOverwrite() demoGaitGeneratorGrasplessManipMode() demoGaitGeneratorSetFootStepsWithArms() + demoGaitGeneratorChangeStrideLimitationType() + print >> sys.stderr, "All samplerobot_auto_balancer.py demo time ", (time.time()-start_time), "[s]" if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_carry_object.py b/sample/SampleRobot/samplerobot_carry_object.py index d0457bf6c18..752763089ad 100755 --- a/sample/SampleRobot/samplerobot_carry_object.py +++ b/sample/SampleRobot/samplerobot_carry_object.py @@ -51,6 +51,15 @@ def demoSetParameter(): stp_org.is_ik_enable=[True]*4 hcf.st_svc.setParameter(stp_org) +def defJointGroups (): + rleg_6dof_group = ['rleg', ['RLEG_HIP_R', 'RLEG_HIP_P', 'RLEG_HIP_Y', 'RLEG_KNEE', 'RLEG_ANKLE_P', 'RLEG_ANKLE_R']] + lleg_6dof_group = ['lleg', ['LLEG_HIP_R', 'LLEG_HIP_P', 'LLEG_HIP_Y', 'LLEG_KNEE', 'LLEG_ANKLE_P', 'LLEG_ANKLE_R']] + torso_group = ['torso', ['WAIST_P', 'WAIST_R', 'CHEST']] + head_group = ['head', []] + rarm_group = ['rarm', ['RARM_SHOULDER_P', 'RARM_SHOULDER_R', 'RARM_SHOULDER_Y', 'RARM_ELBOW', 'RARM_WRIST_Y', 'RARM_WRIST_P', 'RARM_WRIST_R']] + larm_group = ['larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']] + return [rleg_6dof_group, lleg_6dof_group, torso_group, head_group, rarm_group, larm_group] + def init (): global hcf, initial_pose, dualarm_via_pose, dualarm_reach_pose, dualarm_liftup_pose, singlearm_via_pose, singlearm_reach_pose, singlearm_liftup_pose, dualarm_push_pose hcf = HrpsysConfigurator() @@ -80,6 +89,7 @@ def init (): icp.D_r = 1e5 hcf.ic_svc.setImpedanceControllerParam("rarm", icp) hcf.ic_svc.setImpedanceControllerParam("larm", icp) + hcf.Groups = defJointGroups() hcf.startDefaultUnstableControllers(['rarm', 'larm'], ["rleg", "lleg", "rarm", "larm"]) HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() hcf.rmfo_svc.loadForceMomentOffsetParams(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/ForceSensorOffset_SampleRobot.txt') @@ -147,30 +157,32 @@ def demoSinglearmCarryup (is_walk=True, auto_detecion = True): hcf.waitInterpolation() def objectTurnaroundDetection(max_time = 4.0, max_ref_force = 9.8*6.0, limbs=["rarm", "larm"], axis=[0,0,-1]): - otdp=hcf.ic_svc.getObjectTurnaroundDetectorParam()[1] - otdp.detect_time_thre=0.3 - otdp.start_time_thre=0.3 - otdp.axis=axis - hcf.ic_svc.setObjectTurnaroundDetectorParam(otdp) + octdp=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1] + octdp.detect_time_thre=0.3 + octdp.start_time_thre=0.3 + octdp.axis=axis + hcf.octd_svc.setObjectContactTurnaroundDetectorParam(octdp) if limbs==['rarm']: force = [axis[0]*max_ref_force, axis[1]*max_ref_force, axis[2]*max_ref_force] hcf.seq_svc.setWrenches([0]*18+force+[0,0,0], max_time) else: force = [axis[0]*max_ref_force*0.5, axis[1]*max_ref_force*0.5, axis[2]*max_ref_force*0.5] hcf.seq_svc.setWrenches([0]*12+force+[0]*3+force+[0]*3, max_time) - hcf.ic_svc.startObjectTurnaroundDetection(max_ref_force, max_time+2.0, limbs) + hcf.octd_svc.startObjectContactTurnaroundDetection(max_ref_force, max_time+2.0, limbs) flg = True while flg: - tmpflg = hcf.ic_svc.checkObjectTurnaroundDetection() + tmpflg = hcf.octd_svc.checkObjectContactTurnaroundDetection() #print rtm.readDataPort(hcf.rmfo.port("off_rhsensor")).data, rtm.readDataPort(hcf.rmfo.port("off_lhsensor")).data - print " flag = ", tmpflg, ", forces = ", hcf.ic_svc.getObjectForcesMoments()[1][0], ", moments = ", hcf.ic_svc.getObjectForcesMoments()[2][0] - flg = (tmpflg == OpenHRP.ImpedanceControllerService.MODE_DETECTOR_IDLE) or (tmpflg == OpenHRP.ImpedanceControllerService.MODE_STARTED) + [ret, fv, mv, total, fric_w] = hcf.octd_svc.getObjectForcesMoments() + print " flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w + flg = (tmpflg == OpenHRP.ObjectContactTurnaroundDetectorService.MODE_DETECTOR_IDLE) or (tmpflg == OpenHRP.ObjectContactTurnaroundDetectorService.MODE_STARTED) time.sleep(0.5) - print " flag = ", tmpflg, ", forces = ", hcf.ic_svc.getObjectForcesMoments()[1][0], ", moments = ", hcf.ic_svc.getObjectForcesMoments()[2][0] + [ret, fv, mv, total, fric_w] = hcf.octd_svc.getObjectForcesMoments() + print " flag = ", tmpflg, ", forces = ", fv, ", moments = ", mv, ", total = ", total, ", fric_w = ", fric_w if limbs==['rarm']: - hcf.seq_svc.setWrenches([0]*18+hcf.ic_svc.getObjectForcesMoments()[1][0]+hcf.ic_svc.getObjectForcesMoments()[2][0], 2.0) + hcf.seq_svc.setWrenches([0]*18+fv[0]+mv[0], 2.0) else: - hcf.seq_svc.setWrenches([0]*12+hcf.ic_svc.getObjectForcesMoments()[1][0]+hcf.ic_svc.getObjectForcesMoments()[2][0]+hcf.ic_svc.getObjectForcesMoments()[1][1]+hcf.ic_svc.getObjectForcesMoments()[2][1], 2.0) + hcf.seq_svc.setWrenches([0]*12+fv[1]+mv[1]+fv[0]+mv[0], 2.0) hcf.waitInterpolation() def demoWalk (): diff --git a/sample/SampleRobot/samplerobot_collision_detector.py b/sample/SampleRobot/samplerobot_collision_detector.py index 3581b8f8e2d..c3870f78fd5 100755 --- a/sample/SampleRobot/samplerobot_collision_detector.py +++ b/sample/SampleRobot/samplerobot_collision_detector.py @@ -13,20 +13,36 @@ import socket import time +def vector_equal_eps (vec1, vec2, eps=1e-5): + if len(vec1) == len(vec2): + for e1, e2 in zip(vec1, vec2): + if abs(e1 - e2) > eps: + return False + return True + else: + return False + def init (): - global hcf, init_pose, col_safe_pose, col_fail_pose + global hcf, init_pose, col_safe_pose, col_fail_pose, hrpsys_version hcf = HrpsysConfigurator() hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") init_pose = [0]*29 col_safe_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,0.15708,-0.113446,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0] col_fail_pose = [0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.845363,0.03992,0.250074,-1.32816,0.167513,0.016204,0.0,0.0,-0.349066,0.0,0.820305,-0.471239,0.0,0.523599,0.0,0.0,-1.74533,-0.15708,-0.113446,0.0,0.0,0.0,0.0] + hrpsys_version = hcf.co.ref.get_component_profile().version + print >> sys.stderr, "hrpsys_version = %s"%hrpsys_version # demo functions def demoCollisionCheckSafe (): print >> sys.stderr, "1. CollisionCheck in safe pose" - hcf.seq_svc.setJointAngles(col_safe_pose, 1.0); + hcf.seq_svc.setJointAngles(col_safe_pose, 3.0); hcf.waitInterpolation(); + counter = 0 + while (counter < 20) and (not vector_equal_eps([x / 180 * math.pi for x in hcf.getJointAngles()], col_safe_pose)): + time.sleep(0.2) + counter = counter + 1 + assert(counter != 20) cs=hcf.co_svc.getCollisionStatus()[1] if cs.safe_posture: print >> sys.stderr, " => Safe pose" @@ -34,7 +50,7 @@ def demoCollisionCheckSafe (): def demoCollisionCheckFail (): print >> sys.stderr, "2. CollisionCheck in fail pose" - hcf.seq_svc.setJointAngles(col_fail_pose, 1.0); + hcf.seq_svc.setJointAngles(col_fail_pose, 3.0); hcf.waitInterpolation(); cs=hcf.co_svc.getCollisionStatus()[1] if not cs.safe_posture: @@ -129,5 +145,14 @@ def demo(): demoCollisionDisableEnable() #demoCollisionMask() +def demo_co_loop(): + init() + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.10.0'): + demoCollisionCheckSafe() + demoCollisionCheckFail() + demoCollisionCheckFailWithSetTolerance() + demoCollisionDisableEnable() + if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_emergency_stopper.py b/sample/SampleRobot/samplerobot_emergency_stopper.py index 11d3cda7eaf..7b37214a85d 100755 --- a/sample/SampleRobot/samplerobot_emergency_stopper.py +++ b/sample/SampleRobot/samplerobot_emergency_stopper.py @@ -18,6 +18,9 @@ def init (): hcf = HrpsysConfigurator() hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") + if hcf.es != None: + for sen in filter(lambda x: x.type == "Force", hcf.sensors): + hcf.connectLoggerPort(hcf.es, sen.name+"Out") init_pose = [0]*29 reset_pose = [0.0,-0.772215,0.0,1.8338,-1.06158,0.0,0.523599,0.0,0.0,-2.44346,0.15708,-0.113446,0.637045,0.0,-0.772215,0.0,1.8338,-1.06158,0.0,0.523599,0.0,0.0,-2.44346,-0.15708,-0.113446,-0.637045,0.0,0.0,0.0] wrench_command0 = [0.0]*24 @@ -31,8 +34,17 @@ def arrayDistance (angle1, angle2): def arrayApproxEqual (angle1, angle2, thre=1e-3): return arrayDistance(angle1, angle2) < thre +def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-emergency-stopper-check-param"): + hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) + +def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-emergency-stopper-check-param", save_log=True, rtc_name="es"): + if save_log: + saveLogForCheckParameter(log_fname) + return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + def getWrenchArray (): - return reduce(lambda x,y: x+y, (map(lambda fs : rtm.readDataPort(hcf.es.port(fs+"Out")).data, ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor']))) + saveLogForCheckParameter() + return reduce(lambda x,y: x+y, (map(lambda fs : checkParameterFromLog(fs+"Out", save_log=False), ['lfsensor', 'rfsensor', 'lhsensor', 'rhsensor']))) # demo functions def demoEmergencyStopJointAngle (): @@ -152,7 +164,8 @@ def demoEmergencyStopReleaseWhenDeactivated(): def demo(key_interaction=False): init() - if hrpsys_version >= '315.6.0': + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.6.0'): if key_interaction: demoEmergencyStopJointAngleWithKeyInteracton() else: diff --git a/sample/SampleRobot/samplerobot_impedance_controller.py b/sample/SampleRobot/samplerobot_impedance_controller.py index e225e1ce92b..8ef9b096823 100755 --- a/sample/SampleRobot/samplerobot_impedance_controller.py +++ b/sample/SampleRobot/samplerobot_impedance_controller.py @@ -20,73 +20,117 @@ def init (): hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hrpsys_version = hcf.seq.ref.get_component_profile().version print("hrpsys_version = %s"%hrpsys_version) - -def demo(): - init() # set initial pose from sample/controller/SampleController/etc/Sample.pos initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] hcf.seq_svc.setJointAngles(initial_pose, 2.0) hcf.seq_svc.waitInterpolation() +def demoStartStopIMP (): # 0. startImpedance + stopImpedance python interface print >> sys.stderr, "0. startImpedance + stopImpedance python interface" hcf.startImpedance("rarm") hcf.startImpedance("larm") hcf.stopImpedance("larm") hcf.stopImpedance("rarm") - if hrpsys_version < '315.5.0': - return +def demoGetImpedanceControllerParam (): # 1. Getter check print >> sys.stderr, "1. Getter check" - ret1=hcf.ic_svc.getImpedanceControllerParam("rarm") - if ret1[0]: - print >> sys.stderr, " getImpedanceControllerParam => OK" + all_get_ret = [] + for limb in ["rarm", "larm"]: + all_get_ret.append(hcf.ic_svc.getImpedanceControllerParam(limb)[0]) + print >> sys.stderr, " all_get_ret = ", all_get_ret + assert(all(all_get_ret)) + print >> sys.stderr, " getImpedanceControllerParam => OK" + +def demoSetImpedanceControllerParam (): # 2. Setter check print >> sys.stderr, "2. Setter check" - ret1[1].K_r=1.0 - ret1[1].D_r=2.0 - ret1[1].M_r=0.2 - ret2=hcf.ic_svc.setImpedanceControllerParam("rarm", ret1[1]) - ret3=hcf.ic_svc.getImpedanceControllerParam("rarm") - if ret2: - print >> sys.stderr, " setImpedanceControllerParam => OK" + all_set_ret = [] + all_value_ret = [] + for limb in ["rarm", "larm"]: + [ret1, icp1]=hcf.ic_svc.getImpedanceControllerParam(limb) + icp1.K_r=1.0 + icp1.D_r=2.0 + icp1.M_r=0.2 + all_set_ret.append(hcf.ic_svc.setImpedanceControllerParam(limb, icp1)) + [ret2, icp2]=hcf.ic_svc.getImpedanceControllerParam(limb) + all_value_ret.append((icp1.M_r == icp2.M_r) and (icp1.D_r == icp2.D_r) and (icp1.K_r == icp2.K_r)) + print >> sys.stderr, " all_set_ret = ", all_set_ret, ", all_value_ret = ", all_value_ret + assert(all(all_set_ret) and all(all_value_ret)) + print >> sys.stderr, " setImpedanceControllerParam => OK" + +def demoStartImpedanceController (): # 3. Start impedance print >> sys.stderr, "3. Start impedance" - ret4=hcf.ic_svc.startImpedanceController("rarm") - if ret4: - print >> sys.stderr, " startImpedanceController => OK" + all_start_ret = [] + all_mode_ret = [] + # start + for limb in ["rarm", "larm"]: + [ret, icp]=hcf.ic_svc.getImpedanceControllerParam(limb) + all_start_ret.append(hcf.ic_svc.startImpedanceControllerNoWait(limb)) + all_mode_ret.append(icp.controller_mode == OpenHRP.ImpedanceControllerService.MODE_IDLE) + # wait and check + for limb in ["rarm", "larm"]: + hcf.ic_svc.waitImpedanceControllerTransition(limb) + [ret, icp]=hcf.ic_svc.getImpedanceControllerParam(limb) + all_mode_ret.append(icp.controller_mode == OpenHRP.ImpedanceControllerService.MODE_IMP) + # "already start" check + for limb in ["rarm", "larm"]: + all_start_ret.append(not hcf.ic_svc.startImpedanceControllerNoWait(limb)) + print >> sys.stderr, " all_start_ret = ", all_start_ret, ", all_mode_ret = ", all_mode_ret + assert(all(all_start_ret) and all(all_mode_ret)) + print >> sys.stderr, " startImpedanceController => OK" + +def demoSetRefForce (): # 4. Set ref force and moment print >> sys.stderr, "4. Set ref force and moment" hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, - 20,10,-10,0,0,0,], 2.0); + 20,10,-10,0,0,0,], 1.0); hcf.seq_svc.waitInterpolation(); time.sleep(2) hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, - 0,0,0,0,0,0,], 2.0); + 0,0,0,0,0,0,], 1.0); hcf.seq_svc.waitInterpolation(); time.sleep(2) hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, - 0,0,0,0.1,-0.1,0.1], 2.0); + 0,0,0,0.1,-0.1,0.1], 1.0); hcf.seq_svc.waitInterpolation(); time.sleep(2) hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, - 0,0,0,0,0,0], 2.0); + 0,0,0,0,0,0], 1.0); hcf.seq_svc.waitInterpolation(); time.sleep(2) + +def demoStopImpedanceController (): # 5. Stop impedance print >> sys.stderr, "5. Stop impedance" - ret5=hcf.ic_svc.stopImpedanceController("rarm") - if ret5: - print >> sys.stderr, " stopImpedanceController => OK" + all_stop_ret = [] + all_mode_ret = [] + # stop + for limb in ["rarm", "larm"]: + all_stop_ret.append(hcf.ic_svc.stopImpedanceControllerNoWait(limb)) + # wait and check + for limb in ["rarm", "larm"]: + hcf.ic_svc.waitImpedanceControllerTransition(limb) + [ret, icp]=hcf.ic_svc.getImpedanceControllerParam(limb) + all_mode_ret.append(icp.controller_mode == OpenHRP.ImpedanceControllerService.MODE_IDLE) + # "already stop" check + for limb in ["rarm", "larm"]: + all_stop_ret.append(not hcf.ic_svc.stopImpedanceControllerNoWait(limb)) + print >> sys.stderr, " all_stop_ret = ", all_stop_ret, ", all_mode_ret = ", all_mode_ret + assert(all(all_stop_ret) and all(all_mode_ret)) + print >> sys.stderr, " stopImpedanceController => OK" + +def demoArmTrackingCheck (): # 6. Arm tracking check print >> sys.stderr, "6. Arm tracking check" hcf.ic_svc.startImpedanceController("rarm") @@ -94,6 +138,8 @@ def demo(): hcf.waitInterpolation() hcf.setJointAngle("RARM_ELBOW", -70.0, 0.5); hcf.waitInterpolation() + +def demoWorldFrameCheck (): # 7. World frame check if hcf.kinematics_only_mode: print >> sys.stderr, "7. World frame check" @@ -112,6 +158,8 @@ def demo(): hcf.seq_svc.waitInterpolation() else: print >> sys.stderr, "7. World frame check is not executed in non-kinematics-only-mode" + +def demoWorldFrameRefForceCheck (): # 8. World frame ref-force check if hcf.kinematics_only_mode: print >> sys.stderr, "8. World frame ref-force check" @@ -125,7 +173,7 @@ def demo(): hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, - -40,0,0,0,0,0], 2.0); + -40,0,0,0,0,0], 1.0); hcf.seq_svc.waitInterpolation(); hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, @@ -137,7 +185,7 @@ def demo(): hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0, - -40,0,0,0,0,0], 2.0); + -40,0,0,0,0,0], 1.0); hcf.seq_svc.waitInterpolation(); hcf.seq_svc.setWrenches([0,0,0,0,0,0, 0,0,0,0,0,0, @@ -149,5 +197,41 @@ def demo(): else: print >> sys.stderr, "8. World frame ref-force check is not executed in non-kinematics-only-mode" +def demoOCTDCheck (): + # 1. Object Contact Turnaround Detector set param check + print >> sys.stderr, "1. Object Contact Turnaround Detector set param check" + ret9 = True + detect_time_thre = 0.3 + start_time_thre=0.3 + for number_disturbance in [0, 1e-5, -1e-5]: # 1e-5 is smaller than dt + octdp=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1]; + octdp.detect_time_thre = detect_time_thre + number_disturbance + octdp.start_time_thre = start_time_thre + number_disturbance + hcf.octd_svc.setObjectContactTurnaroundDetectorParam(octdp); + octdp2=hcf.octd_svc.getObjectContactTurnaroundDetectorParam()[1]; + print >> sys.stderr, " ", octdp2 + ret9 = ret9 and (octdp2.detect_time_thre == detect_time_thre and octdp2.start_time_thre == start_time_thre) + assert(ret9) + print >> sys.stderr, " => OK" + + +def demo(): + init() + + demoStartStopIMP () + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'): + return + + demoGetImpedanceControllerParam() + demoSetImpedanceControllerParam() + demoStartImpedanceController() + demoSetRefForce() + demoStopImpedanceController() + demoArmTrackingCheck() + demoWorldFrameCheck() + demoWorldFrameRefForceCheck() + demoOCTDCheck() + if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_kalman_filter.py b/sample/SampleRobot/samplerobot_kalman_filter.py index bf13e4a42f7..0b9405cf37d 100755 --- a/sample/SampleRobot/samplerobot_kalman_filter.py +++ b/sample/SampleRobot/samplerobot_kalman_filter.py @@ -65,10 +65,6 @@ def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] test_motion_func() hcf.log_svc.save("/tmp/test-kf-samplerobot-{0}".format(optional_out_file_name)) # Parse data - # Estimated rpy from KF : time, roll, pitch, yaw - estimated_rpy_ret=[] - for line in open("/tmp/test-kf-samplerobot-{0}.kf_rpy".format(optional_out_file_name), "r"): - estimated_rpy_ret.append(line.split(" ")[0:-1]) # Estimated base link rpy from KF : time, roll, pitch, yaw estimated_base_rpy_ret=[] for line in open("/tmp/test-kf-samplerobot-{0}.kf_baseRpyCurrent".format(optional_out_file_name), "r"): @@ -87,14 +83,13 @@ def test_kf_plot (test_motion_func, optional_out_file_name): # time [s] color_list = ['r', 'g', 'b'] for idx in range(3): plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+3+idx]) / math.pi, act_rpy_ret), color=color_list[idx]) - plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+idx]) / math.pi, estimated_rpy_ret), ":", color=color_list[idx]) plt.plot(tm_list, map(lambda x : 180.0 * float(x[1+idx]) / math.pi, estimated_base_rpy_ret), "--", color=color_list[idx]) plt.xlabel("Time [s]") plt.ylabel("Angle [deg]") plt.title("KF actual-estimated data (motion time = {0})".format(optional_out_file_name)) - plt.legend(("Actual roll", "Estimated roll", "Estimated base roll", - "Actual pitch", "Estimated pitch", "Estimated base pitch", - "Actual yaw", "Estimated yaw", "Estimated base yaw")) + plt.legend(("Actual roll", "Estimated base roll", + "Actual pitch", "Estimated base pitch", + "Actual yaw", "Estimated base yaw")) plt.savefig("/tmp/test-kf-samplerobot-data-{0}.eps".format(optional_out_file_name)) except: print >> sys.stderr, "No plot" @@ -157,38 +152,45 @@ def demoSetKalmanFilterParameter(): def demo(): init() - if hrpsys_version >= '315.5.0': + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoGetKalmanFilterParameter() demoSetKalmanFilterParameter() # 3. check log and plot - hcf.kf_svc.resetKalmanFilterState() - hcf.seq_svc.setJointAngles(pitch_poses[0], 1.0) - hcf.seq_svc.waitInterpolation() - test_kf_plot(test_pitch_bending_2s, "pitch-bending-2s") - - #hcf.kf_svc.resetKalmanFilterState() - hcf.seq_svc.setJointAngles(roll_poses[0], 1.0) - hcf.seq_svc.waitInterpolation() - test_kf_plot(test_roll_bending_2s, "roll-bending-2s") - - #hcf.kf_svc.resetKalmanFilterState() - hcf.seq_svc.setJointAngles(yaw_poses[0], 1.0) - hcf.seq_svc.waitInterpolation() - test_kf_plot(test_yaw_bending_2s, "yaw-bending-4s") - - #hcf.kf_svc.resetKalmanFilterState() - hcf.seq_svc.setJointAngles(roll_pitch_poses[0], 1.0) - hcf.seq_svc.waitInterpolation() - test_kf_plot(test_roll_pitch_bending_4s, "roll-pitch-bending-4s") - - hcf.seq_svc.setJointAngles(initial_pose, 1.0) - #hcf.seq_svc.waitInterpolation() - #hcf.kf_svc.resetKalmanFilterState() - #hcf.seq_svc.setJointAngles(initial_pose, 1.0) hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]) - hcf.seq_svc.waitInterpolation() - test_kf_plot(test_walk, "test_walk") + + for alg in [OpenHRP.KalmanFilterService.RPYKalmanFilter, OpenHRP.KalmanFilterService.QuaternionExtendedKalmanFilter]: + kfp=hcf.kf_svc.getKalmanFilterParam()[1] + kfp.kf_algorithm = alg + hcf.kf_svc.setKalmanFilterParam(kfp) + + hcf.kf_svc.resetKalmanFilterState() + hcf.seq_svc.setJointAngles(pitch_poses[0], 1.0) + hcf.seq_svc.waitInterpolation() + test_kf_plot(test_pitch_bending_2s, "pitch-bending-2s"+str(alg)) + + #hcf.kf_svc.resetKalmanFilterState() + hcf.seq_svc.setJointAngles(roll_poses[0], 1.0) + hcf.seq_svc.waitInterpolation() + test_kf_plot(test_roll_bending_2s, "roll-bending-2s"+str(alg)) + + #hcf.kf_svc.resetKalmanFilterState() + hcf.seq_svc.setJointAngles(yaw_poses[0], 1.0) + hcf.seq_svc.waitInterpolation() + test_kf_plot(test_yaw_bending_2s, "yaw-bending-4s"+str(alg)) + + #hcf.kf_svc.resetKalmanFilterState() + hcf.seq_svc.setJointAngles(roll_pitch_poses[0], 1.0) + hcf.seq_svc.waitInterpolation() + test_kf_plot(test_roll_pitch_bending_4s, "roll-pitch-bending-4s"+str(alg)) + + hcf.seq_svc.setJointAngles(initial_pose, 1.0) + #hcf.seq_svc.waitInterpolation() + #hcf.kf_svc.resetKalmanFilterState() + #hcf.seq_svc.setJointAngles(initial_pose, 1.0) + hcf.seq_svc.waitInterpolation() + test_kf_plot(test_walk, "test_walk"+str(alg)) if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_reference_force_updater.py b/sample/SampleRobot/samplerobot_reference_force_updater.py new file mode 100755 index 00000000000..d4249cc7b2f --- /dev/null +++ b/sample/SampleRobot/samplerobot_reference_force_updater.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python + +try: + from hrpsys.hrpsys_config import * + import OpenHRP +except: + print "import without hrpsys" + import rtm + from rtm import * + from OpenHRP import * + import waitInput + from waitInput import * + import socket + import time + +import sys + +def init (): + global hcf, init_pose, reset_pose, wrench_command0, wrench_command1, hrpsys_version + hcf = HrpsysConfigurator() + hcf.getRTCList = hcf.getRTCListUnstable + hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") + hrpsys_version = hcf.seq.ref.get_component_profile().version + print("hrpsys_version = %s"%hrpsys_version) + if hcf.rfu != None: + hcf.connectLoggerPort(hcf.rfu, 'ref_rhsensorOut') + hcf.connectLoggerPort(hcf.rfu, 'ref_lhsensorOut') + +# demo functions +def demoGetReferecenForceUpdateParam (): + print >> sys.stderr, "1. getParam" + for limb_name in ['rarm', 'larm']: + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam(limb_name) + assert(ret) + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm2') # Invalid name + assert(not ret) + print >> sys.stderr, " =>OK" + +def demoSetReferecenForceUpdateParam (): + print >> sys.stderr, "2. setParam" + print >> sys.stderr, " Valid limb access" + for limb_name in ['rarm', 'larm']: + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam(limb_name) + ret = hcf.rfu_svc.setReferenceForceUpdaterParam(limb_name, rfup) + assert(ret) + print >> sys.stderr, " Invalid limb access" + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm2', rfup) # Invalid name + assert(not ret) + print >> sys.stderr, " =>OK" + +def demoReferenceForceUpdater (): + print >> sys.stderr, "3. Reference Force Update" + import numpy as np + i=1; + #print >> sys.stderr, i,". get param";i+=1 + p = hcf.rfu_svc.getReferenceForceUpdaterParam('larm')[1] + p.update_freq=100 + p.p_gain=0.1 + # set rmfo + r_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("rhsensor")[1] + r_fmop.link_offset_centroid = [0,0.0368,-0.076271] + r_fmop.link_offset_mass = 0.80011 + l_fmop = hcf.rmfo_svc.getForceMomentOffsetParam("lhsensor")[1] + l_fmop.link_offset_centroid = [0,-0.0368,-0.076271] + l_fmop.link_offset_mass = 0.80011 + # Set param + hcf.rmfo_svc.setForceMomentOffsetParam("rhsensor", r_fmop) + hcf.rmfo_svc.setForceMomentOffsetParam("lhsensor", l_fmop) + for armName,portName in zip(['rarm', 'larm'],['ref_rhsensorOut','ref_lhsensorOut']): + hcf.rfu_svc.setReferenceForceUpdaterParam(armName,p) + print >> sys.stderr, " 3."+str(i)+". set ref_force from seq [10,0,0]";i+=1 + # set ref_force from seq + hcf.seq_svc.setWrenches([0]*12+[10,0,0,0,0,0]*2,1);time.sleep(1) + portData=checkDataPortFromLog(portName) + print >> sys.stderr, portName,portData[0:3] + ret = np.linalg.norm(portData) > 9.9; + assert (ret) + # start/stop rfu + print >> sys.stderr, " 3."+str(i)+". start/stop param for " + armName; i+=1 + ##start rfu + hcf.rfu_svc.startReferenceForceUpdater(armName);time.sleep(1) + portData=checkDataPortFromLog(portName) + print >> sys.stderr, portName,portData[0:3] + ret = np.linalg.norm(portData) < 0.1; + assert (ret) + ##stop rfu + hcf.rfu_svc.stopReferenceForceUpdater(armName);time.sleep(1) + portData=checkDataPortFromLog(portName) + print >> sys.stderr, portName,portData[0:3] + ret = np.linalg.norm(portData) > 9.9; + assert (ret) + # reset ref_force from seq + print >> sys.stderr, " 3."+str(i)+". set ref_force from seq [0,0,0]";i+=1 + hcf.seq_svc.setWrenches([0]*24,1);time.sleep(1) + +def demoSetReferecenForceUpdateParamWhileActive (): + print >> sys.stderr, "4. setParam while active" + print >> sys.stderr, " 4.0 Start" + hcf.rfu_svc.startReferenceForceUpdater('rarm') + print >> sys.stderr, " 4.1 Check setParam without any changes" + [ret, rfup_org] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup_org) + assert(ret) + print >> sys.stderr, " 4.2 Check setParam which cannot be changed while active" + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') + rfup.frame = 'world' + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + assert(not ret) + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') + rfup.update_freq = rfup.update_freq*10 + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + assert(not ret) + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') + rfup.update_time_ratio = rfup.update_time_ratio*10 + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + assert(not ret) + print >> sys.stderr, " 4.3 Check setParam which can be changed while active" + [ret, rfup] = hcf.rfu_svc.getReferenceForceUpdaterParam('rarm') + rfup.motion_dir = tmp_value = [0,0,-1] + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + print >> sys.stderr, " motion_dir ..." + assert(ret and (map (lambda x,y : abs(x-y)<1e-5, hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].motion_dir, tmp_value))) + rfup.p_gain = tmp_value = rfup.p_gain*0.1 + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + print >> sys.stderr, " p_gain ..." + assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].p_gain-tmp_value) < 1e-5)) + rfup.d_gain = tmp_value = rfup.d_gain*0.1 + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + print >> sys.stderr, " d_gain ..." + assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].d_gain-tmp_value) < 1e-5)) + rfup.i_gain = rfup.i_gain*0.1 + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + print >> sys.stderr, " i_gain ..." + assert(ret and (abs(hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].i_gain-tmp_value) < 1e-5)) + rfup.is_hold_value = tmp_value = not rfup.is_hold_value + ret = hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup) + print >> sys.stderr, " is_hold_value ..." + assert(ret and hcf.rfu_svc.getReferenceForceUpdaterParam('rarm')[1].is_hold_value == tmp_value) + print >> sys.stderr, " 4.4 Stop" + hcf.rfu_svc.stopReferenceForceUpdater('rarm') + hcf.rfu_svc.setReferenceForceUpdaterParam('rarm', rfup_org) + print >> sys.stderr, " =>OK" + +def demoReferecenForceUpdateParamFootOriginExtMoment (): + print >> sys.stderr, "5. FootOriginExtMoment" + ret = hcf.rfu_svc.startReferenceForceUpdater('footoriginextmoment') + ret = hcf.rfu_svc.stopReferenceForceUpdater('footoriginextmoment') and ret + assert(ret) + print >> sys.stderr, " =>OK" + +def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-reference-force-updater-check-port"): + hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) + +def checkDataPortFromLog(port_name, log_fname="/tmp/test-samplerobot-reference-force-updater-check-port",save_log=True, rtc_name="rfu"): + if save_log: + saveLogForCheckParameter(log_fname) + return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + +def demo(): + init() + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.9.0'): + demoGetReferecenForceUpdateParam() + demoSetReferecenForceUpdateParam() + demoReferenceForceUpdater() + if StrictVersion(hrpsys_version) >= StrictVersion('315.13.0'): + demoSetReferecenForceUpdateParamWhileActive() + demoReferecenForceUpdateParamFootOriginExtMoment() + +if __name__ == '__main__': + demo() diff --git a/sample/SampleRobot/samplerobot_remove_force_offset.py b/sample/SampleRobot/samplerobot_remove_force_offset.py index 15022d9aecd..35b7213e9c0 100755 --- a/sample/SampleRobot/samplerobot_remove_force_offset.py +++ b/sample/SampleRobot/samplerobot_remove_force_offset.py @@ -25,6 +25,14 @@ def init (): hrpsys_version = hcf.seq.ref.get_component_profile().version print("hrpsys_version = %s"%hrpsys_version) +def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-remove-force-offset-check-param"): + hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) + +def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-remove-force-offset-check-param", save_log=True, rtc_name="rmfo"): + if save_log: + saveLogForCheckParameter(log_fname) + return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) + def demoGetForceMomentOffsetParam (): print >> sys.stderr, "1. GetForceMomentOffsetParam" for fs_name in ["rhsensor", "lhsensor"]: @@ -36,8 +44,9 @@ def demoGetForceMomentOffsetParam (): def demoSetForceMomentOffsetParam (): print >> sys.stderr, "2. SetForceMomentOffsetParam" print >> sys.stderr, " Force and moment are large because of link offsets" + saveLogForCheckParameter() for fs_name in ["rhsensor", "lhsensor"]: - fm = numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_"+fs_name)).data) + fm = numpy.linalg.norm(checkParameterFromLog("off_"+fs_name, save_log=False)) vret = fm > 5e-2 print >> sys.stderr, " no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret assert(vret) @@ -62,8 +71,9 @@ def demoSetForceMomentOffsetParam (): print >> sys.stderr, " getForceMomentOffsetParam('lhsensor') => OK" assert((ret[0] and ret[1].link_offset_mass == l_fmop.link_offset_mass and ret[1].link_offset_centroid == l_fmop.link_offset_centroid)) print >> sys.stderr, " Force and moment are reduced" + saveLogForCheckParameter() for fs_name in ["rhsensor", "lhsensor"]: - fm = numpy.linalg.norm(rtm.readDataPort(hcf.rmfo.port("off_"+fs_name)).data) + fm = numpy.linalg.norm(checkParameterFromLog("off_"+fs_name, save_log=False)) vret = fm < 5e-2 print >> sys.stderr, " no-offset-removed force moment (",fs_name,") ", fm, "=> ", vret assert(vret) @@ -108,13 +118,25 @@ def demoDumpLoadForceMomentOffsetParams(): print >> sys.stderr, " loadForceMomentOffsetParams => OK" assert((ret and vcheck)) +def demoRemoveForceSensorOffsetRMFO(): + print >> sys.stderr, "4. remove force sensor offset" + print >> sys.stderr, " Test valid calibration" + ret = hcf.removeForceSensorOffsetRMFO(tm=1.0) # all sensors by default + print >> sys.stderr, " Test invalid calibration" + ret = ret and not hcf.removeForceSensorOffsetRMFO(["testtest"], 1.0) # invalid sensor name + if ret: + print >> sys.stderr, " removeforcesensorlinkoffset => OK" + assert(ret) + def demo(): import numpy init() - if hrpsys_version >= '315.5.0': + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoGetForceMomentOffsetParam() demoSetForceMomentOffsetParam() demoDumpLoadForceMomentOffsetParams() + demoRemoveForceSensorOffsetRMFO() if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_sequence_player.py b/sample/SampleRobot/samplerobot_sequence_player.py index fe41495b29b..b43db9a155d 100755 --- a/sample/SampleRobot/samplerobot_sequence_player.py +++ b/sample/SampleRobot/samplerobot_sequence_player.py @@ -13,11 +13,14 @@ import socket import time +from distutils.version import StrictVersion + def init (): global hcf, hrpsys_version hcf = HrpsysConfigurator() hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") hcf.connectLoggerPort(hcf.sh, 'optionalDataOut') # Just for checking + hcf.connectLoggerPort(hcf.el, 'servoStateOut') # Just for checking global reset_pose_doc, move_base_pose_doc, doc # doc for patterns. # torque and wrenches are non-realistic values, just for testing. @@ -62,26 +65,43 @@ def checkArrayEquality (arr1, arr2, eps=1e-7): def checkArrayBetween (arr1, arr2, arr3, eps=1e-7): return all(map(lambda x,y,z : (z-y)*(x-y) <= eps, arr1, arr2, arr3)) +def clearLogForCheckParameter(log_length=1): + hcf.setMaxLogLength(log_length) + hcf.clearLog() + def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-sequence-player-check-param"): - hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) + clearLogForCheckParameter(log_length=1) + time.sleep(0.1);hcf.saveLog(log_fname) def checkParameterFromLog(port_name, log_fname="/tmp/test-samplerobot-sequence-player-check-param", save_log=True, rtc_name="sh"): if save_log: saveLogForCheckParameter(log_fname) return map(float, open(log_fname+"."+rtc_name+"_"+port_name, "r").readline().split(" ")[1:-1]) -def checkJointAngles (var_doc): +def checkServoStateFromLog(log_fname="/tmp/test-samplerobot-sequence-player-check-param"): + hcf.saveLog(log_fname) + ret = True + fp = open(log_fname+'.el_servoStateOut', "r") + for l in fp: + for s in map(int, l.split()[1:-1]): + if s != 7: + print l + ret = False + return ret + +def checkJointAngles (var_doc, eps=1e-7): if isinstance(var_doc, list): p = var_doc else: p = var_doc['pos'] - ret = checkArrayEquality(hcf.sh_svc.getCommand().jointRefs, p) + ret = checkArrayEquality(hcf.sh_svc.getCommand().jointRefs, p, eps) print " pos => ", ret + assert(ret is True) -def checkJointAnglesBetween(from_doc, to_doc): +def checkJointAnglesBetween(from_doc, to_doc, eps=1e-7): p0 = from_doc if isinstance(from_doc, list) else from_doc['pos'] p1 = to_doc if isinstance( to_doc, list) else to_doc['pos'] - ret = checkArrayBetween(p0, hcf.sh_svc.getCommand().jointRefs, p1) + ret = checkArrayBetween(p0, hcf.sh_svc.getCommand().jointRefs, p1, eps) print " pos => ", ret assert(ret is True) @@ -120,7 +140,7 @@ def checkRobotState (var_doc): checkZmp(var_doc) checkWaist(var_doc) checkTorque(var_doc, save_log=False) - if hrpsys_version >= '315.2.0': + if StrictVersion(hrpsys_version) >= StrictVersion('315.2.0'): checkWrenches(var_doc, save_log=False) checkOptionalData(var_doc, save_log=False) @@ -141,7 +161,7 @@ def demoSetJointAngles(): hcf.seq_svc.waitInterpolation(); checkJointAngles(reset_pose_doc) # check clear - if hrpsys_version < '315.5.0': + if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'): return print >> sys.stderr, " check clear" hcf.seq_svc.setJointAngles(move_base_pose_doc['pos'], 5.0); @@ -268,14 +288,28 @@ def demoSetJointAnglesOfGroup(): hcf.seq_svc.waitInterpolationOfGroup('larm'); checkJointAngles(p1) # check clear - if hrpsys_version < '315.5.0': + if StrictVersion(hrpsys_version) < StrictVersion('315.5.0'): return - print >> sys.stderr, " check clear" + print >> sys.stderr, " check clear (clearJointAnglesOfGroup)" hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0); time.sleep(2.5) hcf.seq_svc.clearJointAnglesOfGroup('larm') checkJointAnglesBetween(p1, p0) + hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 5.0); + hcf.seq_svc.waitInterpolationOfGroup('larm') + checkJointAngles(p1) + + print >> sys.stderr, " check clear clearOfGroup" + hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 5.0); + time.sleep(2.5) + hcf.seq_svc.clearOfGroup('larm', 0.0) + checkJointAnglesBetween(p1, p0) + + hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 5.0); + hcf.seq_svc.waitInterpolationOfGroup('larm') + checkJointAngles(p1) + def demoSetJointAnglesSequenceOfGroup(): print >> sys.stderr, "9. setJointAnglesOfGroup" hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']) @@ -377,22 +411,100 @@ def demoSetJointAnglesSequenceFull(): hcf.seq_svc.clearJointAngles() checkJointAnglesBetween(reset_pose_doc,move_base_pose_doc) +def demoSetTargetPose(): + # reset wait position + hcf.seq_svc.setBasePos([0.000000, 0.000000, 0.723500], 1.0); + hcf.seq_svc.setBaseRpy([0.000000, 0.000000, 0.000000], 1.0); + print >> sys.stderr, "11. setTargetPose" + hcf.seq_svc.addJointGroup('larm', ['LARM_SHOULDER_P', 'LARM_SHOULDER_R', 'LARM_SHOULDER_Y', 'LARM_ELBOW', 'LARM_WRIST_Y', 'LARM_WRIST_P', 'LARM_WRIST_R']) + larm_pos0 = [-0.000111, 0.31129, -0.159481, -1.57079, -0.636277, 0.0, 0.0] + larm_pos1 = [-0.000111, 0.31129, -0.159481, -0.785395, -0.636277, 0.0, 0.0] + hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); + hcf.seq_svc.waitInterpolation(); + hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos0, 1.0); + hcf.seq_svc.waitInterpolationOfGroup('larm'); + pos0 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST') + rpy0 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST') + pos0_world = hcf.getCurrentPosition('LARM_WRIST_R') + rpy0_world = hcf.getCurrentRPY('LARM_WRIST_R') + p0 = list(reset_pose_doc['pos']) # copy + for i in range(len(larm_pos0)): + p0[i+19] = larm_pos0[i] + checkJointAngles(p0) + + hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0); + hcf.seq_svc.waitInterpolationOfGroup('larm'); + pos1 = hcf.getCurrentPosition('LARM_WRIST_R:WAIST') + rpy1 = hcf.getCurrentRPY('LARM_WRIST_R:WAIST') + pos1_world = hcf.getCurrentPosition('LARM_WRIST_R') + rpy1_world = hcf.getCurrentRPY('LARM_WRIST_R') + p1 = list(reset_pose_doc['pos']) # copy + for i in range(len(larm_pos1)): + p1[i+19] = larm_pos1[i] + checkJointAngles(p1) + + print >> sys.stderr, " check setTargetPose" + hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0) + hcf.seq_svc.waitInterpolationOfGroup('larm'); + print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0) + checkJointAngles(p0, 0.01) + + clearLogForCheckParameter(5000) + hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0) + hcf.seq_svc.waitInterpolationOfGroup('larm'); + print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1) + checkJointAngles(p1, 0.01) + assert(checkServoStateFromLog() is True) + + hcf.seq_svc.setJointAnglesOfGroup('larm', larm_pos1, 1.0); + hcf.seq_svc.waitInterpolationOfGroup('larm'); + + print >> sys.stderr, " check setTargetPose without giving reference frame" + hcf.seq_svc.setTargetPose('larm', pos0_world, rpy0_world, 2.0) + hcf.seq_svc.waitInterpolationOfGroup('larm'); + print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p0) + checkJointAngles(p0, 0.01) + + clearLogForCheckParameter(5000) + hcf.seq_svc.setTargetPose('larm', pos1_world, rpy1_world, 2.0) + hcf.seq_svc.waitInterpolationOfGroup('larm'); + print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1) + checkJointAngles(p1, 0.01) + assert(checkServoStateFromLog() is True) + + print >> sys.stderr, " check clear clearOfGroup" + hcf.seq_svc.setTargetPose('larm:WAIST', pos0, rpy0, 2.0) + time.sleep(0.5) + hcf.seq_svc.clearOfGroup('larm', 0.0) + checkJointAnglesBetween(p0, p1, 0.01) + + clearLogForCheckParameter(5000) + hcf.seq_svc.setTargetPose('larm:WAIST', pos1, rpy1, 2.0) + hcf.seq_svc.waitInterpolationOfGroup('larm') + print map(lambda x,y : abs(x-y), hcf.sh_svc.getCommand().jointRefs, p1) + checkJointAngles(p1, 0.1) + assert(checkServoStateFromLog() is True) + hcf.seq_svc.removeJointGroup('larm') + hcf.seq_svc.setJointAngles(reset_pose_doc['pos'], 1.0); + hcf.seq_svc.waitInterpolation(); def demo(): init() demoSetJointAngles() - if hrpsys_version >= '315.5.0': + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoSetJointAnglesSequence() demoSetJointAngle() demoLoadPattern() demoSetZmp() demoSetBasePosRpy() - if hrpsys_version >= '315.2.0': + if StrictVersion(hrpsys_version) >= StrictVersion('315.2.0'): demoSetWrenches() demoSetJointAnglesOfGroup() - if hrpsys_version >= '315.5.0': + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoSetJointAnglesSequenceOfGroup() demoSetJointAnglesSequenceFull() + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): + demoSetTargetPose() if __name__ == '__main__': demo() diff --git a/sample/SampleRobot/samplerobot_soft_error_limiter.py b/sample/SampleRobot/samplerobot_soft_error_limiter.py index 36885d3bf5a..766c9dfc59c 100755 --- a/sample/SampleRobot/samplerobot_soft_error_limiter.py +++ b/sample/SampleRobot/samplerobot_soft_error_limiter.py @@ -41,7 +41,8 @@ def init (): def demo (): init() - if hrpsys_version >= '315.5.0': + from distutils.version import StrictVersion + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoTestAllLimitTables() demoPositionLimit() demoVelocityLimit() @@ -215,12 +216,12 @@ def setAndCheckJointErrorLimit (joint_name, thre=1e-5): for is_upper_limit in [True, False]: # uvlimit or lvlimit print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit" # Disable error limit for checking vel limit - error_limit = 1.0 if is_upper_limit else -1.0 # [deg] - hcf.el_svc.setServoErrorLimit("all", abs(math.radians(error_limit))) + error_limit = 0.002 if is_upper_limit else -0.002 # [rad] + hcf.el_svc.setServoErrorLimit("all", abs(error_limit)) # Test motion and logging hcf.clearLog() target_angle = 3.0 if is_upper_limit else -3.0 # [deg] - wait_time = abs(target_angle/error_limit * 1.1) # 1.1 is margin + wait_time = 0.2 hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1) hcf.waitInterpolation() hcf.setJointAngle(joint_name, target_angle, 0.002) @@ -238,16 +239,19 @@ def setAndCheckJointErrorLimit (joint_name, thre=1e-5): poslist=[] for line in open("/tmp/test-samplerobot-el-err-check.SampleRobot(Robot)0_q", "r"): poslist.append(float(line.split(" ")[link_info.jointId+1])) - tmp = map(lambda x,y : x-y, poslist[1:], poslist[0:-1]) + refposlist=[] + for line in open("/tmp/test-samplerobot-el-err-check.el_q", "r"): + refposlist.append(float(line.split(" ")[link_info.jointId+1])) + tmp = map(lambda x,y : x-y, refposlist, poslist) max_ret_err = max(tmp) if is_upper_limit else min(tmp) - is_err_limited = abs(max_ret_err - math.radians(error_limit)) < thre + is_err_limited = abs(max_ret_err - error_limit) < thre # Enable error limit by reverting limit value and reset joint angle hcf.el_svc.setServoErrorLimit("all", (0.2-0.02)) hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5) hcf.waitInterpolation() # Check flags and print print >> sys.stderr, " is_reached =", is_reached, ", is_err_limited =", is_err_limited, - print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_err =", max_ret_err, "[rad], err_limit =", math.radians(error_limit), "[rad]" + print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_err =", max_ret_err, "[rad], err_limit =", error_limit, "[rad]" assert(is_reached and is_err_limited) def demoErrorLimit(): diff --git a/sample/SampleRobot/samplerobot_stabilizer.py b/sample/SampleRobot/samplerobot_stabilizer.py index 74e46d1bd14..ae97eede37b 100755 --- a/sample/SampleRobot/samplerobot_stabilizer.py +++ b/sample/SampleRobot/samplerobot_stabilizer.py @@ -15,21 +15,22 @@ import math from subprocess import check_output +from distutils.version import StrictVersion def init (): - global hcf, initial_pose, hrpsys_version + global hcf, initial_pose, half_sitting_pose, hrpsys_version hcf = HrpsysConfigurator() hcf.getRTCList = hcf.getRTCListUnstable hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl") # set initial pose from sample/controller/SampleController/etc/Sample.pos initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0] - hcf.seq_svc.setJointAngles(initial_pose, 2.0) - hcf.seq_svc.waitInterpolation() + half_sitting_pose = [-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000158,-0.570987,-0.000232,1.26437,-0.692521,0.000277,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] hrpsys_version = hcf.seq.ref.get_component_profile().version print("hrpsys_version = %s"%hrpsys_version) - if hrpsys_version >= '315.5.0': - # Start AutoBalancer - hcf.startAutoBalancer() + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): + # on < 315.5.0 this outputs huge error log message + hcf.seq_svc.setJointAngles(initial_pose, 2.0) + hcf.seq_svc.waitInterpolation() # Remove offset for sen in ["rfsensor", "lfsensor"]: ofp=hcf.rmfo_svc.getForceMomentOffsetParam(sen)[1]; @@ -80,6 +81,9 @@ def demoSetParameter(): stp_org.eefm_k3=[-0.175068,-0.175068] stp_org.eefm_rot_damping_gain = [[20*1.6*1.5, 20*1.6*1.5, 1e5]]*4 stp_org.eefm_pos_damping_gain = [[3500*50, 3500*50, 3500*1.0*1.5]]*4 + stp_org.eefm_swing_rot_damping_gain = stp_org.eefm_rot_damping_gain[0] + stp_org.eefm_swing_pos_damping_gain = stp_org.eefm_pos_damping_gain[0] + stp_org.eefm_use_swing_damping=True hcf.st_svc.setParameter(stp_org) stp = hcf.st_svc.getParameter() vcheck = stp.k_tpcc_p == stp_org.k_tpcc_p and stp.k_tpcc_x == stp_org.k_tpcc_x and stp.k_brot_p == stp_org.k_brot_p @@ -87,6 +91,17 @@ def demoSetParameter(): print >> sys.stderr, " setParameter() => OK", vcheck assert(vcheck) +def changeContactDecisionThre (thre): + stp = hcf.st_svc.getParameter() + stp.contact_decision_threshold=thre + hcf.st_svc.setParameter(stp) + +def mimicInTheAir (): + changeContactDecisionThre(10000) # [N] + +def mimicOnTheGround (): + changeContactDecisionThre(50) # [N], default + def saveLogForCheckParameter(log_fname="/tmp/test-samplerobot-stabilizer-check-param"): hcf.setMaxLogLength(1);hcf.clearLog();time.sleep(0.1);hcf.saveLog(log_fname) @@ -103,19 +118,65 @@ def checkActualBaseAttitude(thre=5.0): # degree print >> sys.stderr, " ret = ", ret, ", actual base rpy = (", act_rpy, ")" return ret -def demoStartStopTPCCST (): - print >> sys.stderr, "3. start and stop TPCC st" +def printActualBase(): + '''Print actual base pos and rot + ''' + act_base = checkParameterFromLog("WAIST") + print >> sys.stderr, " actual base pos = ", act_base[0:3], "[m], actual base rpy = ", act_base[3:], "[rad]" + +def changeSTAlgorithm (new_st_alg): + stp = hcf.st_svc.getParameter() + if stp.st_algorithm != new_st_alg: + hcf.stopStabilizer() + stp.st_algorithm = new_st_alg + hcf.st_svc.setParameter(stp) + hcf.startStabilizer () + # Wait for osscilation being samall + hcf.setJointAngles(hcf.getJointAngles(), 2.0); + hcf.waitInterpolation() + +def demoSTLoadPattern (): + print >> sys.stderr, "3. EEFMQP st + SequencePlayer loadPattern" if hcf.pdc: + # Set initial pose of samplerobot-gopos000 before starting of ST + hcf.seq_svc.setJointAnglesSequenceFull([[0.000242, -0.403476, -0.000185, 0.832071, -0.427767, -6.928952e-05, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, 0.000242, -0.403469, -0.000185, 0.832073, -0.427775, -6.928781e-05, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # jvss + [[0]*29], # vels + [[0]*29], # torques + [[-0.014759, -4.336272e-05, 0.668138]], # poss + [[-0.000245, -0.000862, 0.000171]], # rpys + [[0]*3], # accs + [[0.014052, 0.000203, -0.66798]], # zmps + [[0]*6*4], # wrenchs + [[1,1,0,0,1,1,1,1]], # optionals + [0.5]); # tms + hcf.stopAutoBalancer() + hcf.seq_svc.waitInterpolation() stp = hcf.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.TPCC + stp.emergency_check_mode=OpenHRP.StabilizerService.NO_CHECK # Disable checking of emergency error because currently this error checker does not work correctly during walking. hcf.st_svc.setParameter(stp) + #changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP) + changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQP) + hcf.startStabilizer() + # Exec loadPattern + HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() + hcf.loadPattern(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/data/samplerobot-gopos000', 0.0) + hcf.waitInterpolation() + ret = checkActualBaseAttitude() + if ret: + print >> sys.stderr, " ST + loadPattern => OK" + assert(ret) + else: + print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + +def demoStartStopTPCCST (): + print >> sys.stderr, "4. start and stop TPCC st" + if hcf.pdc: + # setup controllers + hcf.startAutoBalancer() + changeSTAlgorithm (OpenHRP.StabilizerService.TPCC) hcf.startStabilizer () #hcf.abc_svc.goPos(0.5, 0.1, 10) #hcf.abc_svc.waitFootSteps() - hcf.stopStabilizer () - # Wait for non-st osscilation being samall - hcf.seq_svc.setJointAngles(initial_pose, 2.0) - hcf.waitInterpolation() ret = checkActualBaseAttitude() if ret: print >> sys.stderr, " Start and Stop Stabilizer => OK" @@ -123,20 +184,15 @@ def demoStartStopTPCCST (): else: print >> sys.stderr, " This sample is neglected in High-gain mode simulation" - def demoStartStopEEFMQPST (): - print >> sys.stderr, "4. start and stop EEFMQP st" + print >> sys.stderr, "5. start and stop EEFMQP st" if hcf.pdc: - stp = hcf.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP - hcf.st_svc.setParameter(stp) - hcf.startStabilizer () + # setup controllers + hcf.startAutoBalancer() + changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQP) + hcf.startStabilizer() hcf.abc_svc.goPos(0.3, 0, 0) hcf.abc_svc.waitFootSteps() - hcf.stopStabilizer () - # Wait for non-st osscilation being samall - hcf.seq_svc.setJointAngles(initial_pose, 2.0) - hcf.waitInterpolation() ret = checkActualBaseAttitude() if ret: print >> sys.stderr, " Start and Stop Stabilizer => OK" @@ -144,60 +200,124 @@ def demoStartStopEEFMQPST (): else: print >> sys.stderr, " This sample is neglected in High-gain mode simulation" -def demoSTLoadPattern (): - print >> sys.stderr, "5. EEFMQP st + SequencePlayer loadPattern" +def demoSTStairWalk (): + print >> sys.stderr, "6. EEFMQPCOP + stair" if hcf.pdc: - stp = hcf.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP - stp.emergency_check_mode=OpenHRP.StabilizerService.NO_CHECK # Disable checking of emergency error because currently this error checker does not work correctly during walking. - hcf.st_svc.setParameter(stp) - hcf.stopAutoBalancer() - # Set initial pose of samplerobot-gopos000 before starting of ST - hcf.seq_svc.setJointAnglesSequenceFull([[0.000242, -0.403476, -0.000185, 0.832071, -0.427767, -6.928952e-05, 0.31129, -0.159481, -0.115399, -0.636277, 0.0, 0.0, 0.0, 0.000242, -0.403469, -0.000185, 0.832073, -0.427775, -6.928781e-05, 0.31129, 0.159481, 0.115399, -0.636277, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]], # jvss - [[0]*29], # vels - [[0]*29], # torques - [[-0.014759, -4.336272e-05, 0.668138]], # poss - [[-0.000245, -0.000862, 0.000171]], # rpys - [[0]*3], # accs - [[0.014052, 0.000203, -0.66798]], # zmps - [[0]*6*4], # wrenchs - [[1,1,0,0,1,1,1,1]], # optionals - [2.0]); # tms - hcf.seq_svc.waitInterpolation() - hcf.startStabilizer () - # Exec loadPattern - HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip() - hcf.loadPattern(HRPSYS_DIR+'/share/hrpsys/samples/SampleRobot/data/samplerobot-gopos000', 0.0) - hcf.waitInterpolation() - hcf.stopStabilizer () - # Wait for non-st osscilation being samall - hcf.seq_svc.setJointAngles(initial_pose, 2.0) - hcf.waitInterpolation() + # setup controllers + printActualBase() + changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP) + hcf.startStabilizer() + hcf.startAutoBalancer() + hcf.seq_svc.setJointAngles(half_sitting_pose, 1.0); + hcf.waitInterpolation(); + printActualBase() + # set gg param + ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + org_ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + ggp.default_orbit_type = OpenHRP.AutoBalancerService.STAIR + ggp.swing_trajectory_time_offset_xy2z=0.1 + ggp.swing_trajectory_delay_time_offset=0.2 + ggp.toe_heel_phase_ratio=[0.05, 0.25, 0.20, 0.0, 0.18, 0.23, 0.09] + ggp.toe_pos_offset_x = 1e-3*182.0; + ggp.heel_pos_offset_x = 1e-3*-72.0; + ggp.toe_zmp_offset_x = 1e-3*182.0; + ggp.heel_zmp_offset_x = 1e-3*-72.0; + ggp.use_toe_heel_transition=True + ggp.use_toe_heel_auto_set = True + ggp.toe_angle = 20; + ggp.heel_angle = 10; + hcf.abc_svc.setGaitGeneratorParam(ggp) + printActualBase() + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.27,0.09,0.1], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.27,-0.09,0.1], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.54,0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.54,-0.09,0], [1,0,0,0], "rleg")])]); + printActualBase() + hcf.abc_svc.waitFootSteps(); + printActualBase() + # finish + hcf.abc_svc.setGaitGeneratorParam(org_ggp) ret = checkActualBaseAttitude() + printActualBase() if ret: - print >> sys.stderr, " ST + loadPattern => OK" + print >> sys.stderr, " ST + Stair => OK" + assert(ret) + else: + print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + +def demoSTToeHeelWalk (): + print >> sys.stderr, "7. EEFMQPCOP + toeheel" + if hcf.pdc: + # setup controllers + hcf.startAutoBalancer() + hcf.co_svc.disableCollisionDetection() + changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP) + hcf.startStabilizer() + hcf.seq_svc.setJointAngles(initial_pose, 2.0); + hcf.waitInterpolation(); + # set gg param + ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + org_ggp = hcf.abc_svc.getGaitGeneratorParam()[1] + ggp.default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE + ggp.swing_trajectory_time_offset_xy2z=0.1 + ggp.swing_trajectory_delay_time_offset=0.2 + ggp.toe_heel_phase_ratio=[0.05, 0.35, 0.20, 0.0, 0.13, 0.13, 0.14] + ggp.toe_pos_offset_x = 1e-3*182.0; + ggp.heel_pos_offset_x = 1e-3*-72.0; + ggp.toe_zmp_offset_x = 1e-3*182.0; + ggp.heel_zmp_offset_x = 1e-3*-72.0; + ggp.use_toe_heel_transition=True + ggp.use_toe_heel_auto_set=True + # test setFootStepsWithParam + ggp.default_double_support_ratio=0.7 + hcf.abc_svc.setGaitGeneratorParam(ggp) + for sgn in [1, -1]: + hcf.setFootStepsWithParam([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([sgn*0.22,0.09,0], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([sgn*0.44,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([sgn*0.44,0.09,0], [1,0,0,0], "lleg")])], + [OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.0, step_time=1.0, toe_angle=0.0, heel_angle=0.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=4.0, toe_angle=20.0, heel_angle=10.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=4.0, toe_angle=20.0, heel_angle=10.0)]), + OpenHRP.AutoBalancerService.StepParams([OpenHRP.AutoBalancerService.StepParam(step_height=0.05, step_time=4.0, toe_angle=20.0, heel_angle=10.0)])]) + hcf.abc_svc.waitFootSteps(); + # test goPos + ggp.default_double_support_ratio=0.2 + ggp.stride_parameter=[0.22,0.1,20.0,0.22] + ggp.toe_angle = 20; + ggp.heel_angle = 10; + hcf.abc_svc.setGaitGeneratorParam(ggp) + for sgn in [1, -1]: + hcf.abc_svc.goPos(sgn*0.66,sgn*0.2,sgn*40); + hcf.abc_svc.waitFootSteps(); + # finish + hcf.abc_svc.setGaitGeneratorParam(org_ggp) + hcf.co_svc.enableCollisionDetection() + ret = checkActualBaseAttitude() + if ret: + print >> sys.stderr, " ST + ToeHeel => OK" assert(ret) else: print >> sys.stderr, " This sample is neglected in High-gain mode simulation" def demoSTTurnWalk (): - print >> sys.stderr, "6. EEFMQP st + Turn walk" + print >> sys.stderr, "8. EEFMQPCOP st + Turn walk" if hcf.pdc: - stp = hcf.st_svc.getParameter() - stp.st_algorithm=OpenHRP.StabilizerService.EEFMQP - hcf.st_svc.setParameter(stp) hcf.startAutoBalancer() + hcf.co_svc.disableCollisionDetection() + changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP) + hcf.startStabilizer() ggp = hcf.abc_svc.getGaitGeneratorParam()[1] org_ggp = hcf.abc_svc.getGaitGeneratorParam()[1] ggp.stride_parameter=[0.15, 0.15, 90.0, 0.05] hcf.abc_svc.setGaitGeneratorParam(ggp) - hcf.co_svc.disableCollisionDetection() - hcf.startStabilizer () + hcf.abc_svc.goPos(0,-0.2,0); + hcf.abc_svc.waitFootSteps(); hcf.abc_svc.goPos(0,0,175); hcf.abc_svc.waitFootSteps(); hcf.abc_svc.goPos(0.4,0.15,40); hcf.abc_svc.waitFootSteps(); - hcf.stopStabilizer () # Wait for non-st osscilation being samalpl hcf.abc_svc.setGaitGeneratorParam(org_ggp) hcf.co_svc.enableCollisionDetection() @@ -208,17 +328,125 @@ def demoSTTurnWalk (): else: print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + +def demoSTTransitionAirGround (): + # This example is from YoheiKakiuchi's comment : https://github.com/fkanehiro/hrpsys-base/issues/1098, https://github.com/fkanehiro/hrpsys-base/pull/1102#issuecomment-284609203 + print >> sys.stderr, "9. ST Transition (in the air and on the ground)" + if hcf.pdc: + # Init + stp_org = hcf.st_svc.getParameter() + stp = hcf.st_svc.getParameter() + stp.transition_time = 0.1; # for fast checking + hcf.st_svc.setParameter(stp) + # Tests + print >> sys.stderr, " 9-1. Check in the air" + hcf.startStabilizer() + mimicInTheAir() + hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition + cmode1 = hcf.st_svc.getParameter().controller_mode + vcheck1 = (cmode1 == OpenHRP.StabilizerService.MODE_AIR) + print >> sys.stderr, " 9-2. Check on the ground" + mimicOnTheGround() + hcf.setJointAngles(hcf.getJointAngles(), stp.transition_time);hcf.waitInterpolation() # Wait transition + cmode2 = hcf.st_svc.getParameter().controller_mode + vcheck2 = (cmode2 == OpenHRP.StabilizerService.MODE_ST) + print >> sys.stderr, " 9-3. Check in the air and then stopST" + mimicInTheAir() + hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until in the air flag is invoked in onExecute + hcf.stopStabilizer() + cmode3 = hcf.st_svc.getParameter().controller_mode + vcheck3 = (cmode3 == OpenHRP.StabilizerService.MODE_IDLE) + print >> sys.stderr, " 9-4. Check on the ground" + mimicOnTheGround() + hcf.setJointAngles(hcf.getJointAngles(), 0.01);hcf.waitInterpolation() # Wait until on the ground flag is invoked in onExecute + hcf.startStabilizer() + cmode4 = hcf.st_svc.getParameter().controller_mode + vcheck4 = (cmode4 == OpenHRP.StabilizerService.MODE_ST) + # Finsh + hcf.st_svc.setParameter(stp_org) + vcheck_list = [vcheck1, vcheck2, vcheck3, vcheck4] + print >> sys.stderr, " ST Transition Air Ground vcheck = ", vcheck_list, ", cmode = ", [cmode1, cmode2, cmode3, cmode4] + if all(vcheck_list): + print >> sys.stderr, " ST Transition Air Ground => OK" + assert(all(vcheck_list)) + else: + print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + +def demoSTRootRotChange (): + print >> sys.stderr, "10. ST root rot change" + if hcf.pdc: + # 10deg + root_rot_x_pose=[-0.240857,-0.634561,0.012382,1.30211,-0.669201,0.073893,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.232865,-0.555515,0.011753,1.1356,-0.581653,0.06476,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] + # 35deg + root_rot_y_pose=[-1.706033e-05,-1.04708,-0.000479,0.497763,-0.060719,-0.000105,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-1.690260e-05,-1.04693,-0.000479,0.497318,-0.060422,-0.000105,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] + # 25deg + root_rot_z_pose=[-0.261382,-0.479591,-0.490714,1.26471,-0.722778,0.018041,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.313108,-0.610397,-0.535653,1.24943,-0.571839,-0.013257,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] + # all 10deg + root_rot_xyz_pose=[-0.378611,-0.81283,-0.238181,1.23534,-0.577915,0.061071,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.351695,-0.768514,-0.225097,1.05221,-0.442267,0.050849,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] + hcf.startAutoBalancer(); + changeSTAlgorithm (OpenHRP.StabilizerService.EEFMQPCOP) + print >> sys.stderr, " init" + checkActualBaseAttitude() + hcf.seq_svc.setJointAngles(root_rot_x_pose, 1.0);hcf.waitInterpolation(); + hcf.seq_svc.setJointAngles(root_rot_x_pose, 1.0);hcf.waitInterpolation(); # dummy for wait + print >> sys.stderr, " root rot x done." + checkActualBaseAttitude() + hcf.seq_svc.setJointAngles(root_rot_y_pose, 1.0);hcf.waitInterpolation(); + hcf.seq_svc.setJointAngles(root_rot_y_pose, 1.0);hcf.waitInterpolation(); # dummy for wait + print >> sys.stderr, " root rot y done." + checkActualBaseAttitude() + hcf.seq_svc.setJointAngles(root_rot_z_pose, 1.0);hcf.waitInterpolation(); + hcf.seq_svc.setJointAngles(root_rot_z_pose, 1.0);hcf.waitInterpolation(); # dummy for wait + print >> sys.stderr, " root rot z done." + checkActualBaseAttitude() + hcf.seq_svc.setJointAngles(root_rot_xyz_pose, 1.0);hcf.waitInterpolation(); + hcf.seq_svc.setJointAngles(root_rot_xyz_pose, 1.0);hcf.waitInterpolation(); # dummy for wait + hcf.seq_svc.setJointAngles(initial_pose, 1.0);hcf.waitInterpolation(); + print >> sys.stderr, " root rot xyz done." + ret = checkActualBaseAttitude() + if ret: + print >> sys.stderr, " ST root rot change => OK" + assert(ret) + else: + print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + +def demoSTMimicRouchTerrainWalk (terrain_height_diff = 0.04): + print >> sys.stderr, "11. ST mimic rough terrain walk" + if hcf.pdc: + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.22,0.09,terrain_height_diff], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,0.09,0], [1,0,0,0], "lleg")])]); + hcf.abc_svc.waitFootSteps(); + hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.22,0.09,-1*terrain_height_diff], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,-0.09,0], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.44,0.09,0], [1,0,0,0], "lleg")])]); + hcf.abc_svc.waitFootSteps(); + ret = checkActualBaseAttitude() + if ret: + print >> sys.stderr, " ST mimic rough terrain walk => OK" + assert(ret) + else: + print >> sys.stderr, " This sample is neglected in High-gain mode simulation" + + def demo(): OPENHRP3_DIR=check_output(['pkg-config', 'openhrp3.1', '--variable=prefix']).rstrip() if os.path.exists(OPENHRP3_DIR+"/share/OpenHRP-3.1/sample/model/sample1_bush.wrl"): init() - if hrpsys_version >= '315.5.0': + if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'): demoGetParameter() demoSetParameter() + demoSTLoadPattern() demoStartStopTPCCST() demoStartStopEEFMQPST() - demoSTLoadPattern() + demoSTStairWalk() + demoSTToeHeelWalk() demoSTTurnWalk() + demoSTTransitionAirGround() + demoSTRootRotChange() + demoSTMimicRouchTerrainWalk() else: print >> sys.stderr, "Skip st test because of missing sample1_bush.wrl" diff --git a/sample/SampleRobot/samplerobot_terrain_walk.py b/sample/SampleRobot/samplerobot_terrain_walk.py index ea8d15bb2a2..5a9f474c66e 100755 --- a/sample/SampleRobot/samplerobot_terrain_walk.py +++ b/sample/SampleRobot/samplerobot_terrain_walk.py @@ -24,22 +24,20 @@ def initPose(): initial_pose=[-0.000181,-0.614916,-0.000239,1.36542,-0.749643,0.000288,0.31129,-0.159481,-0.115399,-0.636277,0.0,0.0,0.0,-0.000181,-0.614916,-0.000239,1.36542,-0.749643,0.000288,0.31129,0.159481,0.115399,-0.636277,0.0,0.0,0.0,0.0,0.0,0.0] hcf.seq_svc.setJointAngles(initial_pose, 1.0) hcf.seq_svc.waitInterpolation() + hcf.startAutoBalancer(); def demo(): init() initPose() - print "Please execute 'demoStairUp()', 'demoStairDown()', 'demoStairUpDown()', and 'demoSlopeUpDown()'" -def setupGaitGeneratorParam(use_rectangle=True): +def setupGaitGeneratorParam(set_step_height=False): ggp = hcf.abc_svc.getGaitGeneratorParam() ggp[1].default_double_support_ratio = 0.3 ggp[1].default_step_time = 1.2 + if set_step_height: + ggp[1].default_step_height = 0.1 #ggp[1].swing_trajectory_delay_time_offset = 0.2; - if use_rectangle: - ggp[1].default_orbit_type = OpenHRP.AutoBalancerService.RECTANGLE; - else: - ggp[1].default_orbit_type = OpenHRP.AutoBalancerService.STAIR; - #ggp[1].stair_trajectory_way_point_offset = [0.04,0,0]; + ggp[1].default_orbit_type = OpenHRP.AutoBalancerService.STAIR; hcf.abc_svc.setGaitGeneratorParam(ggp[1]) def stairWalk(stair_height = 0.1524): @@ -47,25 +45,22 @@ def stairWalk(stair_height = 0.1524): floor_stride_x = 0.16 init_step_x = 0 init_step_z = 0 + ret = [] + setupGaitGeneratorParam() for step_idx in [1,2,3,4]: - setupGaitGeneratorParam(step_idx%2==1) - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x, -0.09, init_step_z], [1,0,0,0], "rleg")]), - OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])]) - hcf.abc_svc.waitFootSteps() - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]), - OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")])]) - hcf.abc_svc.waitFootSteps() - hcf.setFootSteps([OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]), - OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")]), - OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])]) - hcf.abc_svc.waitFootSteps() + ret = ret + [OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x, -0.09, init_step_z], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x, -0.09, init_step_z+stair_height], [1,0,0,0], "rleg")]), + OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([init_step_x+stair_stride_x+floor_stride_x, 0.09, init_step_z+stair_height], [1,0,0,0], "lleg")])] init_step_x = init_step_x + stair_stride_x + floor_stride_x init_step_z = init_step_z + stair_height + hcf.setFootSteps(ret) + hcf.abc_svc.waitFootSteps() # sample for SampleRobot.TerrainFloor.SlopeUpDown.xml def demoSlopeUpDown(): print "Start stlop up down" - setupGaitGeneratorParam() + setupGaitGeneratorParam(True) hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]); fsList=[OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([0.8,-0.09,0.0], [1.0,0.0,2.775558e-17,0.0], "rleg")]), OpenHRP.AutoBalancerService.Footsteps([OpenHRP.AutoBalancerService.Footstep([1.0953,0.09,0.030712], [0.991445,0.0,-0.130526,0.0], "lleg")]), @@ -92,26 +87,22 @@ def demoSlopeUpDown(): # sample for SampleRobot.TerrainFloor.StairUp.xml def demoStairUp(): print "Start stair up" - hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]); stairWalk() - hcf.abc_svc.stopAutoBalancer(); # sample for SampleRobot.TerrainFloor.StairDown.xml def demoStairDown(): print "Start stair down" - hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]); + hcf.abc_svc.goPos(0.05, 0.0, 0.0) + hcf.abc_svc.waitFootSteps() stairWalk(-0.1524) - hcf.abc_svc.stopAutoBalancer(); def demoStairUpDown(): print "Start stair up" - hcf.abc_svc.startAutoBalancer(["rleg", "lleg"]); stairWalk() hcf.abc_svc.goPos(0.05, 0.0, 0.0) hcf.abc_svc.waitFootSteps() print "Start stair down" stairWalk(-0.1524) - hcf.abc_svc.stopAutoBalancer(); if __name__ == '__main__': demo() diff --git a/test/test-hrpsysconf.py b/test/test-hrpsysconf.py index a84e36060cc..aa501e580d8 100755 --- a/test/test-hrpsysconf.py +++ b/test/test-hrpsysconf.py @@ -23,12 +23,29 @@ def init(self, robotname="SampleRobot(Robot)0", url=""): class TestHrpsysConfig(unittest.TestCase): global h + rh = None + seq = None def test_import_waitinput(self): # https://github.com/start-jsk/rtmros_hironx/blob/groovy-devel/hironx_ros_bridge/src/hironx_ros_bridge/hironx_client.py from waitInput import waitInputConfirm, waitInputSelect self.assertTrue(True) + def test_createcomp(self): + global h + self.seq = h.createComp("SequencePlayer",'seq')[0] + + def test_connectcomp(self): + global h + if self.seq == None or self.rh == None: + self.test_createcomp() + connectPorts(self.rh.port("q"), self.seq.port("qInit")) + # check number of connection + assert(len(self.seq.port("qInit").get_connector_profiles()) == 1) + # check do not connect again if already connected for https://github.com/fkanehiro/hrpsys-base/issues/979 + connectPorts(self.rh.port("q"), self.seq.port("qInit")) + assert(len(self.seq.port("qInit").get_connector_profiles()) == 1) + def test_findcomp(self): global h h.findComps() @@ -46,6 +63,13 @@ def setUp(self): rtm.nsport = args.port h = SampleHrpsysConfigurator() + h.waitForRTCManager() + # look for name + for c in h.ms.get_components(): + if '(Robot)' in c.name() or 'RobotHardware' in c.name(): + h.waitForRobotHardware(c.name()) # get robot hardware name + break; + self.rh = h.rh #unittest.main() if __name__ == '__main__': diff --git a/test/test-samplerobot-co-init_collision.test b/test/test-samplerobot-co-init_collision.test new file mode 100644 index 00000000000..466505be9ea --- /dev/null +++ b/test/test-samplerobot-co-init_collision.test @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/test/test-samplerobot-co_loop-init_collision.test b/test/test-samplerobot-co_loop-init_collision.test new file mode 100644 index 00000000000..fc7e17548b8 --- /dev/null +++ b/test/test-samplerobot-co_loop-init_collision.test @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/test/test-samplerobot-co_loop.test b/test/test-samplerobot-co_loop.test new file mode 100644 index 00000000000..72374f69c95 --- /dev/null +++ b/test/test-samplerobot-co_loop.test @@ -0,0 +1,10 @@ + + + + + + + + + diff --git a/test/test-samplerobot-collision.py b/test/test-samplerobot-collision.py index b12ce6be0cb..47363c4a5b1 100755 --- a/test/test-samplerobot-collision.py +++ b/test/test-samplerobot-collision.py @@ -9,9 +9,14 @@ import samplerobot_collision_detector import unittest, rostest -class TestSampleRobotCollisionDetector(unittest.TestCase): - def test_demo (self): - samplerobot_collision_detector.demo() +if [s for s in sys.argv if '__name:=samplerobot_co_loop' in s]: + class TestSampleRobotCollisionDetector(unittest.TestCase): + def test_demo (self): + samplerobot_collision_detector.demo_co_loop() +else: + class TestSampleRobotCollisionDetector(unittest.TestCase): + def test_demo (self): + samplerobot_collision_detector.demo() ## IGNORE ME: this code used for rostest if [s for s in sys.argv if "--gtest_output=xml:" in s] : diff --git a/test/test-samplerobot-rfu.py b/test/test-samplerobot-rfu.py new file mode 100755 index 00000000000..961821d6906 --- /dev/null +++ b/test/test-samplerobot-rfu.py @@ -0,0 +1,18 @@ +#!/usr/bin/env python + +import imp, sys, os, time + +# set path to hrpsys to use HrpsysConfigurator +from subprocess import check_output +sys.path.append(os.path.join(check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip(),'share/hrpsys/samples/SampleRobot/')) # set path to SampleRobot + +import samplerobot_reference_force_updater +import unittest, rostest + +class TestSampleRobotReferenceForceUpdater(unittest.TestCase): + def test_demo (self): + samplerobot_reference_force_updater.demo() + +## IGNORE ME: this code used for rostest +if [s for s in sys.argv if "--gtest_output=xml:" in s] : + rostest.run('hrpsys', 'samplerobot_reference_force_updater', TestSampleRobotReferenceForceUpdater, sys.argv) diff --git a/test/test-samplerobot.test b/test/test-samplerobot.test index fc5a1e606b9..64e382a0123 100644 --- a/test/test-samplerobot.test +++ b/test/test-samplerobot.test @@ -29,4 +29,6 @@ args="-ORBInitRef NameService=corbaloc:iiop:localhost:2809/NameService" retry="2" time-limit="600"/> + \ No newline at end of file diff --git a/util/CMakeLists.txt b/util/CMakeLists.txt index 8e3d27f423f..8a533d0c282 100644 --- a/util/CMakeLists.txt +++ b/util/CMakeLists.txt @@ -1,4 +1,5 @@ add_subdirectory(ProjectGenerator) +add_subdirectory(SelfCollisionChecker) if(USE_HRPSYSUTIL) add_subdirectory(simulator) add_subdirectory(viewer) diff --git a/util/ProjectGenerator/CMakeLists.txt b/util/ProjectGenerator/CMakeLists.txt index b20e549ce14..c1d6c5682a4 100644 --- a/util/ProjectGenerator/CMakeLists.txt +++ b/util/ProjectGenerator/CMakeLists.txt @@ -5,5 +5,5 @@ target_link_libraries(ProjectGenerator ${OPENHRP_LIBRARIES} ${LIBXML2_LIBRARIES} set(target ProjectGenerator) install(TARGETS ${target} - RUNTIME DESTINATION bin CONFIGURATIONS Release Debug + RUNTIME DESTINATION bin ) diff --git a/util/ProjectGenerator/ProjectGenerator.cpp b/util/ProjectGenerator/ProjectGenerator.cpp index 60bd4cd54f2..44bebf2bb32 100644 --- a/util/ProjectGenerator/ProjectGenerator.cpp +++ b/util/ProjectGenerator/ProjectGenerator.cpp @@ -177,7 +177,7 @@ int main (int argc, char** argv) xmlTextWriterWriteProperty(writer, "outport", "tau:JOINT_TORQUE"); // set outport for sensros - int nforce = body->numSensors(hrp::Sensor::FORCE); + unsigned int nforce = body->numSensors(hrp::Sensor::FORCE); if ( nforce > 0 ) std::cerr << "hrp::Sensor::FORCE"; for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i); @@ -186,7 +186,7 @@ int main (int argc, char** argv) std::cerr << " " << s->name; } if ( nforce > 0 ) std::cerr << std::endl; - int ngyro = body->numSensors(hrp::Sensor::RATE_GYRO); + unsigned int ngyro = body->numSensors(hrp::Sensor::RATE_GYRO); if ( ngyro > 0 ) std::cerr << "hrp::Sensor::GYRO"; if(ngyro == 1){ // port is named with no number when there is only one gyro @@ -203,7 +203,7 @@ int main (int argc, char** argv) } } if ( ngyro > 0 ) std::cerr << std::endl; - int nacc = body->numSensors(hrp::Sensor::ACCELERATION); + unsigned int nacc = body->numSensors(hrp::Sensor::ACCELERATION); if ( nacc > 0 ) std::cerr << "hrp::Sensor::ACCELERATION"; if(nacc == 1){ // port is named with no number when there is only one acc @@ -253,7 +253,7 @@ int main (int argc, char** argv) joint_properties_map.insert(std::pair(joint_properties_arg_str[i*2], joint_properties_arg_str[i*2+1])); } if ( body->numJoints() > 0 ) std::cerr << "hrp::Joint"; - for(int i = 0; i < body->numJoints(); i++){ + for(unsigned int i = 0; i < body->numJoints(); i++){ if ( body->joint(i)->index > 0 ) { std::cerr << " " << body->joint(i)->name << "(" << body->joint(i)->jointId << ")"; std::string joint_name = body->joint(i)->name; diff --git a/util/SelfCollisionChecker/CMakeLists.txt b/util/SelfCollisionChecker/CMakeLists.txt new file mode 100644 index 00000000000..df0f8c0a6a8 --- /dev/null +++ b/util/SelfCollisionChecker/CMakeLists.txt @@ -0,0 +1,8 @@ +add_executable(hrpsys-self-collision-checker scc.cpp main.cpp) +target_link_libraries(hrpsys-self-collision-checker ${OPENHRP_LIBRARIES}) + +set(target hrpsys-self-collision-checker) + +install(TARGETS ${target} + RUNTIME DESTINATION bin +) diff --git a/util/SelfCollisionChecker/main.cpp b/util/SelfCollisionChecker/main.cpp new file mode 100644 index 00000000000..857a1c9dbdb --- /dev/null +++ b/util/SelfCollisionChecker/main.cpp @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include +#include +#include "scc.h" + +using namespace OpenHRP; +using namespace hrp; + +int main(int argc, char *argv[]) +{ + if (argc < 3){ + std::cerr << "Usage: " << argv[0] << "[VRML model] [log file] [-olv] [linkName1:linkNam2 ...]" + << std::endl; + return 1; + } + + hrp::LinkNamePairList pairs; + bool useOLV=false; + for (int i=3; iload(robot->name().c_str(), argv[1]); + olv->clearLog(); + } + OpenHRP::WorldState wstate; + wstate.characterPositions.length(1); + setupCharacterPosition(wstate.characterPositions[0], robot); + + std::ifstream ifs(argv[2]); + double tm, q[robot->numJoints()]; + + int ret = 0; + ifs >> tm; + while (!ifs.eof()){ + for (unsigned int i=0; inumJoints(); i++){ + ifs >> q[i]; + } + pairs = scc.check(q); + for (unsigned int i=0; iupdate(wstate); + } + ifs >> tm; + } + + return ret; +} diff --git a/util/SelfCollisionChecker/scc.cpp b/util/SelfCollisionChecker/scc.cpp new file mode 100644 index 00000000000..76a870a83d9 --- /dev/null +++ b/util/SelfCollisionChecker/scc.cpp @@ -0,0 +1,52 @@ +#include +#include "scc.h" + +using namespace hrp; + +SelfCollisionChecker::SelfCollisionChecker(hrp::BodyPtr body, const hrp::LinkNamePairList &pairs) : m_robot(body) +{ + for (unsigned int i=0; inumLinks(); i++){ + Link *link1 = m_robot->link(i); + for (unsigned int j=i+1; jnumLinks(); j++){ + Link *link2 = m_robot->link(j); + if (link1->parent != link2 && link2->parent != link1){ + bool skip = false; + for (unsigned int k=0; kname + && pairs[k].second == link2->name) + ||(pairs[k].first == link2->name + && pairs[k].second == link1->name)){ + skip = true; + break; + } + } + if (!skip){ + m_checkPairs.push_back(ColdetModelPair(link1->coldetModel, + link2->coldetModel)); + } + } + } + } +} + + +LinkNamePairList SelfCollisionChecker::check(const double *q) +{ + LinkNamePairList pairs; + + for (unsigned int i=0; inumJoints(); i++){ + m_robot->joint(i)->q = q[i]; + } + m_robot->calcForwardKinematics(); + for (unsigned int i=0; inumLinks(); i++){ + Link *l = m_robot->link(i); + l->coldetModel->setPosition(l->attitude(), l->p); + } + for (unsigned int i=0; iname(), + m_checkPairs[i].model(1)->name())); + } + } + return pairs; +} diff --git a/util/SelfCollisionChecker/scc.h b/util/SelfCollisionChecker/scc.h new file mode 100644 index 00000000000..9242622b1b2 --- /dev/null +++ b/util/SelfCollisionChecker/scc.h @@ -0,0 +1,20 @@ +#include +#include + +namespace hrp{ + +typedef std::vector > LinkNamePairList; + +class SelfCollisionChecker +{ +public: + SelfCollisionChecker(hrp::BodyPtr body, + const LinkNamePairList& pairs=LinkNamePairList()); + LinkNamePairList check(const double *q); + unsigned int numOfCheckPairs() const { return m_checkPairs.size(); } +private: + hrp::BodyPtr m_robot; + std::vector m_checkPairs; +}; + +} diff --git a/util/monitor/GLscene.cpp b/util/monitor/GLscene.cpp index 3b42889b73d..30a4c2f06a7 100644 --- a/util/monitor/GLscene.cpp +++ b/util/monitor/GLscene.cpp @@ -7,10 +7,10 @@ #include #endif #include -#include "util/GLcamera.h" -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/LogManager.h" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/LogManager.h" #include "TimedRobotState.h" #include "GLscene.h" @@ -83,7 +83,7 @@ void GLscene::updateScene() tform[9], tform[10], tform[11]; } if (com.jointRefs.length() == glbody->numJoints()){ - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ GLlink *j = (GLlink *)glbody->joint(i); if (j){ j->q = com.jointRefs[i]; @@ -93,7 +93,7 @@ void GLscene::updateScene() } glbody->calcForwardKinematics(); glbody->updateLinkColdetModelPositions(); - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ ((GLlink *)glbody->link(i))->highlight(false); } for (size_t i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *l = glbody->joint(i); if (l){ int ss = rstate.servoState[i][0]; @@ -159,7 +159,7 @@ void GLscene::showStatus() // driver temperature int temp = temperature(ss); if (!temp){ - sprintf(buf, "--", temp); + sprintf(buf, "--"); }else{ sprintf(buf, "%2d", temp); } diff --git a/util/monitor/GLscene.h b/util/monitor/GLscene.h index f89e99ac5d6..43e4ede9f7e 100644 --- a/util/monitor/GLscene.h +++ b/util/monitor/GLscene.h @@ -2,7 +2,7 @@ #define __GLSCENE_H__ #include -#include "util/GLsceneBase.h" +#include "hrpsys/util/GLsceneBase.h" class LogManagerBase; diff --git a/util/monitor/Monitor.cpp b/util/monitor/Monitor.cpp index 2e1d7f81a8d..f3f5b779fe0 100644 --- a/util/monitor/Monitor.cpp +++ b/util/monitor/Monitor.cpp @@ -1,6 +1,6 @@ #include #include "Monitor.h" -#include "util/OpenRTMUtil.h" +#include "hrpsys/util/OpenRTMUtil.h" #include "GLscene.h" Monitor::Monitor(CORBA::ORB_var orb, const std::string &i_hostname, @@ -8,8 +8,8 @@ Monitor::Monitor(CORBA::ORB_var orb, const std::string &i_hostname, m_orb(orb), m_rhCompName("RobotHardware0"), m_shCompName("StateHolder0"), - m_interval(i_interval), - m_log(i_log) + m_log(i_log), + m_interval(i_interval) { char buf[128]; try { @@ -157,7 +157,7 @@ void Monitor::showStatus(hrp::BodyPtr &body) m_log->tail(); fprintf(stdout, "\e[2KID PW NAME ANGLE COMMAND ERROR VELOCITY ACCEL. TORQUE SERVO TEMP\n"); // greep backgroupd char buf[256]; - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ hrp::Link *l = body->joint(i); if (l){ fprintf(stdout,"\e[2K"); @@ -190,9 +190,9 @@ void Monitor::showStatus(hrp::BodyPtr &body) // error if( i 1 ) yellow(); - if ( abs(e) > 2 ) magenta(); - if ( abs(e) > 4 ) red(); + if ( std::abs(e) > 1 ) yellow(); + if ( std::abs(e) > 2 ) magenta(); + if ( std::abs(e) > 4 ) red(); fprintf(stdout, "%8.3f ", e); black(); }else{ @@ -201,9 +201,9 @@ void Monitor::showStatus(hrp::BodyPtr &body) // velocity if( i 2 ) yellow(); - if ( abs(e) > 10 ) magenta(); - if ( abs(e) > 20 ) red(); + if ( std::abs(e) > 2 ) yellow(); + if ( std::abs(e) > 10 ) magenta(); + if ( std::abs(e) > 20 ) red(); fprintf(stdout, "%8.2f ", e); black(); }else{ @@ -212,9 +212,9 @@ void Monitor::showStatus(hrp::BodyPtr &body) // accleration if( i 50 ) yellow(); - if ( abs(e) > 100 ) magenta(); - if ( abs(e) > 200 ) red(); + if ( std::abs(e) > 50 ) yellow(); + if ( std::abs(e) > 100 ) magenta(); + if ( std::abs(e) > 200 ) red(); fprintf(stdout, "%8.1f ", e); black(); }else{ @@ -231,7 +231,7 @@ void Monitor::showStatus(hrp::BodyPtr &body) // driver temperature int temp = temperature(ss); if (!temp){ - fprintf(stdout, "-- ", temp); + fprintf(stdout, "-- "); }else{ if (temp >= 60) red(); fprintf(stdout, "%2d ", temp); diff --git a/util/monitor/Monitor.h b/util/monitor/Monitor.h index f1c33a86af9..dbf8e621631 100644 --- a/util/monitor/Monitor.h +++ b/util/monitor/Monitor.h @@ -1,7 +1,7 @@ #include -#include "util/ThreadedObject.h" -#include "util/LogManager.h" -#include "StateHolderService.hh" +#include "hrpsys/util/ThreadedObject.h" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/idl/StateHolderService.hh" #include "TimedRobotState.h" #include "hrpModel/Body.h" diff --git a/util/monitor/TimedRobotState.h b/util/monitor/TimedRobotState.h index eea2b634f2c..e6e41d5cdb3 100644 --- a/util/monitor/TimedRobotState.h +++ b/util/monitor/TimedRobotState.h @@ -1,8 +1,8 @@ #ifndef __TIMED_ROBOT_STATE_H__ #define __TIMED_ROBOT_STATE_H__ -#include "RobotHardwareService.hh" -#include "StateHolderService.hh" +#include "hrpsys/idl/RobotHardwareService.hh" +#include "hrpsys/idl/StateHolderService.hh" typedef struct { diff --git a/util/monitor/main.cpp b/util/monitor/main.cpp index f7071a45eb7..1063cbb2acd 100644 --- a/util/monitor/main.cpp +++ b/util/monitor/main.cpp @@ -8,11 +8,11 @@ #else #include #endif -#include "util/ProjectUtil.h" -#include "util/GLbody.h" -#include "util/GLlink.h" -#include "util/GLutil.h" -#include "util/SDLUtil.h" +#include "hrpsys/util/ProjectUtil.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" +#include "hrpsys/util/SDLUtil.h" #include "GLscene.h" #include "Monitor.h" diff --git a/util/simulator/BodyState.cpp b/util/simulator/BodyState.cpp index 27b14ba1dac..169c14e7e7a 100644 --- a/util/simulator/BodyState.cpp +++ b/util/simulator/BodyState.cpp @@ -10,7 +10,7 @@ void BodyState::set(BodyPtr i_body) p = root->p; R = root->attitude(); q.resize(i_body->numLinks()); - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ q[i] = i_body->link(i)->q; } int n; diff --git a/util/simulator/CMakeLists.txt b/util/simulator/CMakeLists.txt index efad6e14d69..1f6c0c607b7 100644 --- a/util/simulator/CMakeLists.txt +++ b/util/simulator/CMakeLists.txt @@ -40,7 +40,7 @@ if (NOT APPLE) ) else() target_link_libraries(hrpsysext - boost_python + boost_python27-mt hrpsysUtil ${PYTHON_LIBRARIES} ) @@ -54,7 +54,7 @@ install(TARGETS ${target} ) install(TARGETS hrpsysext - LIBRARY DESTINATION ${python_dist_pkg_dir}/hrpsys CONFIGURATIONS Release Debug + LIBRARY DESTINATION ${python_dist_pkg_dir}/hrpsys ) install(PROGRAMS diff --git a/util/simulator/GLscene.cpp b/util/simulator/GLscene.cpp index 9d99b7fe9af..1844c82d48a 100644 --- a/util/simulator/GLscene.cpp +++ b/util/simulator/GLscene.cpp @@ -9,10 +9,10 @@ #endif #include #include -#include "util/GLcamera.h" -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/LogManager.h" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/LogManager.h" #include "SceneState.h" #include "GLscene.h" @@ -100,11 +100,11 @@ void GLscene::showStatus() int height = m_height-HEIGHT_STEP; char buf[256]; double q[glbody->numJoints()]; - for (int i=0; inumLinks(); i++){ + for (unsigned int i=0; inumLinks(); i++){ Link* l = glbody->link(i); if (l->jointId >= 0) q[l->jointId] = bstate->q[i]; } - for (int i=0; inumJoints(); i++){ + for (unsigned int i=0; inumJoints(); i++){ GLlink *l = (GLlink *)glbody->joint(i); if (l){ sprintf(buf, "%2d %15s %8.3f", i, l->name.c_str(), @@ -230,7 +230,7 @@ void GLscene::drawSensorOutput(Body *body, Sensor *sensor) bool colored = v->imageType == VisionSensor::COLOR_DEPTH; glBegin(GL_POINTS); float *ptr = (float *)&v->depth[0]; - for (int i=0; idepth.size()/16; i++){ + for (unsigned int i=0; idepth.size()/16; i++){ glVertex3f(ptr[0], ptr[1], ptr[2]); if (colored){ ptr += 3; diff --git a/util/simulator/GLscene.h b/util/simulator/GLscene.h index b0ea4883081..9e620a17201 100644 --- a/util/simulator/GLscene.h +++ b/util/simulator/GLscene.h @@ -1,7 +1,7 @@ #ifndef __GLSCENE_H__ #define __GLSCENE_H__ -#include "util/GLsceneBase.h" +#include "hrpsys/util/GLsceneBase.h" class GLscene : public GLsceneBase { diff --git a/util/simulator/PyBody.cpp b/util/simulator/PyBody.cpp index b7b1dc0f0eb..0aac97548e0 100644 --- a/util/simulator/PyBody.cpp +++ b/util/simulator/PyBody.cpp @@ -45,7 +45,7 @@ void PyBody::setRotation(PyObject *v) void PyBody::setPosture(PyObject *v) { if (PySequence_Size(v) != numJoints()) return; - for (int i=0; iq = boost::python::extract(PySequence_GetItem(v, i)); } @@ -65,7 +65,7 @@ PyObject *PyBody::getRotation() PyObject *PyBody::getPosture() { boost::python::list retval; - for (int i=0; iq : 0; retval.append(boost::python::object(q)); @@ -91,7 +91,7 @@ PyLink *PyBody::link(std::string name) PyObject *PyBody::links() { boost::python::list retval; - for (int i=0; i -#include "util/BodyRTC.h" -#include "util/GLbody.h" +#include "hrpsys/util/BodyRTC.h" +#include "hrpsys/util/GLbody.h" class PyLink; class PySimulator; diff --git a/util/simulator/PyLink.cpp b/util/simulator/PyLink.cpp index 40f21fc7398..56369c1ffe5 100644 --- a/util/simulator/PyLink.cpp +++ b/util/simulator/PyLink.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include "hrpsys/util/GLutil.h" #include "PyLink.h" #include "PyBody.h" #include "PyShape.h" diff --git a/util/simulator/PyLink.h b/util/simulator/PyLink.h index 43a32848909..6fbae2a3ca8 100644 --- a/util/simulator/PyLink.h +++ b/util/simulator/PyLink.h @@ -2,7 +2,7 @@ #define __PYLINK_H__ #include -#include "util/GLlink.h" +#include "hrpsys/util/GLlink.h" class PyShape; diff --git a/util/simulator/PyShape.h b/util/simulator/PyShape.h index 55d98259420..ca76c74a686 100644 --- a/util/simulator/PyShape.h +++ b/util/simulator/PyShape.h @@ -2,7 +2,7 @@ #define __PYSHAPE_H__ #include -#include "util/GLshape.h" +#include "hrpsys/util/GLshape.h" class PyShape : public GLshape { diff --git a/util/simulator/PySimulator.cpp b/util/simulator/PySimulator.cpp index cef4dd4eb34..cb71cb67076 100644 --- a/util/simulator/PySimulator.cpp +++ b/util/simulator/PySimulator.cpp @@ -13,12 +13,12 @@ #include #endif #include -#include "util/GLbody.h" -#include "util/GLlink.h" -#include "util/GLutil.h" -#include "util/Project.h" -#include "util/OpenRTMUtil.h" -#include "util/BVutil.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" +#include "hrpsys/util/Project.h" +#include "hrpsys/util/OpenRTMUtil.h" +#include "hrpsys/util/BVutil.h" #include "PyBody.h" #include "PyLink.h" #include "PyShape.h" @@ -85,14 +85,14 @@ hrp::BodyPtr createBody(const std::string& name, const ModelItem& mitem, } PySimulator::PySimulator() : - manager(NULL), Simulator(&log), scene(&log), window(&scene, &log, this), + Simulator(&log), scene(&log), window(&scene, &log, this), manager(NULL), useBBox(false), maxLogLen(60) { initRTCmanager(); } PySimulator::PySimulator(PyObject *pyo) : - manager(NULL), Simulator(&log), scene(&log), window(&scene, &log, this), + Simulator(&log), scene(&log), window(&scene, &log, this), manager(NULL), useBBox(false) { initRTCmanager(pyo); @@ -195,7 +195,7 @@ bool PySimulator::loadProject(std::string fname){ BodyFactory factory = boost::bind(::createBody, _1, _2, modelloader, &scene, useBBox); init(prj, factory); - for (int i=0; i(body(i).get()); pybody->setListener(this); } @@ -274,7 +274,7 @@ PyBody *PySimulator::createBody(std::string name) PyObject *PySimulator::bodies() { boost::python::list retval; - for (int i=0; i(body(i).get()); retval.append(boost::python::ptr(b)); } @@ -325,7 +325,7 @@ void PySimulator::reset() { log.clear(); setCurrentTime(0.0); - for (int i=0; iinitializeConfiguration(); } checkCollision(); diff --git a/util/simulator/PySimulator.h b/util/simulator/PySimulator.h index cacdcd8a7be..3727cc8395d 100644 --- a/util/simulator/PySimulator.h +++ b/util/simulator/PySimulator.h @@ -1,7 +1,7 @@ #ifndef __PYSIMULATOR_H__ #define __PYSIMULATOR_H__ -#include "util/SDLUtil.h" +#include "hrpsys/util/SDLUtil.h" #include "Simulator.h" #include "GLscene.h" diff --git a/util/simulator/SceneState.cpp b/util/simulator/SceneState.cpp index aae3d6e486c..e8a13b1db95 100644 --- a/util/simulator/SceneState.cpp +++ b/util/simulator/SceneState.cpp @@ -7,7 +7,7 @@ void SceneState::set(hrp::WorldBase& i_world, OpenHRP::CollisionSequence& i_coll { time = i_world.currentTime(); bodyStates.resize(i_world.numBodies()); - for (int i=0; i *i_log) : log(i_log), adjustTime(false) @@ -47,7 +47,7 @@ void Simulator::checkCollision() void Simulator::checkCollision(OpenHRP::CollisionSequence &collisions) { - for (int i=0; iupdateLinkColdetModelPositions(); } for(size_t colIndex=0; colIndex < pairs.size(); ++colIndex){ @@ -60,14 +60,14 @@ void Simulator::checkCollision(OpenHRP::CollisionSequence &collisions) pCollisionPoints->length(0); } else { int npoints = 0; - for(int i = 0; i < cdata.size(); i++) { + for(unsigned int i = 0; i < cdata.size(); i++) { for(int j = 0; j < cdata[i].num_of_i_points; j++){ if(cdata[i].i_point_new[j]) npoints++; } } pCollisionPoints->length(npoints); int idx = 0; - for (int i = 0; i < cdata.size(); i++) { + for (unsigned int i = 0; i < cdata.size(); i++) { hrp::collision_data& cd = cdata[i]; for(int j=0; j < cd.num_of_i_points; j++){ if (cd.i_point_new[j]){ @@ -139,7 +139,7 @@ bool Simulator::oneStep(){ tm_dynamics.begin(); constraintForceSolver.clearExternalForces(); if (m_kinematicsOnly){ - for (int i=0; icalcForwardKinematics(); } currentTime_ += timeStep(); @@ -167,10 +167,10 @@ bool Simulator::oneStep(){ tm_collision.totalTime(), tm_collision.averageTime()*1000); printf("dynamics :%8.3f[s], %8.3f[ms/frame]\n", tm_dynamics.totalTime(), tm_dynamics.averageTime()*1000); - for (int i=0; ibody(i); int ntri=0; - for (int j=0; jnumLinks(); j++){ + for (unsigned int j=0; jnumLinks(); j++){ hrp::Link *l = body->link(j); if (l && l->coldetModel){ ntri += l->coldetModel->getNumTriangles(); diff --git a/util/simulator/Simulator.h b/util/simulator/Simulator.h index 8cd61b46d87..43335b9fd06 100644 --- a/util/simulator/Simulator.h +++ b/util/simulator/Simulator.h @@ -2,10 +2,10 @@ #include #include #include -#include "util/Project.h" -#include "util/ThreadedObject.h" -#include "util/LogManager.h" -#include "util/ProjectUtil.h" +#include "hrpsys/util/Project.h" +#include "hrpsys/util/ThreadedObject.h" +#include "hrpsys/util/LogManager.h" +#include "hrpsys/util/ProjectUtil.h" #include "SceneState.h" class BodyRTC; diff --git a/util/simulator/main.cpp b/util/simulator/main.cpp index c7b3f54cabb..5544794dfca 100644 --- a/util/simulator/main.cpp +++ b/util/simulator/main.cpp @@ -10,13 +10,13 @@ #include #endif #include -#include "util/GLbodyRTC.h" -#include "util/GLlink.h" -#include "util/GLutil.h" -#include "util/Project.h" -#include "util/OpenRTMUtil.h" -#include "util/SDLUtil.h" -#include "util/BVutil.h" +#include "hrpsys/util/GLbodyRTC.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" +#include "hrpsys/util/Project.h" +#include "hrpsys/util/OpenRTMUtil.h" +#include "hrpsys/util/SDLUtil.h" +#include "hrpsys/util/BVutil.h" #include "Simulator.h" #include "GLscene.h" diff --git a/util/viewer/GLscene.cpp b/util/viewer/GLscene.cpp index 286b842eb3b..77fe08891b9 100644 --- a/util/viewer/GLscene.cpp +++ b/util/viewer/GLscene.cpp @@ -7,10 +7,10 @@ #else #include #endif -#include "util/GLcamera.h" -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/LogManager.h" +#include "hrpsys/util/GLcamera.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/LogManager.h" #include "GLscene.h" using namespace OpenHRP; diff --git a/util/viewer/GLscene.h b/util/viewer/GLscene.h index 9bfd4c823ed..dbf51092ef7 100644 --- a/util/viewer/GLscene.h +++ b/util/viewer/GLscene.h @@ -3,7 +3,7 @@ #include #include -#include "util/GLsceneBase.h" +#include "hrpsys/util/GLsceneBase.h" class LogManagerBase; diff --git a/util/viewer/OnlineViewer_impl.cpp b/util/viewer/OnlineViewer_impl.cpp index 639e337d0ae..4b92d4274e0 100644 --- a/util/viewer/OnlineViewer_impl.cpp +++ b/util/viewer/OnlineViewer_impl.cpp @@ -1,9 +1,9 @@ #include #include #include -#include "util/GLbody.h" -#include "util/GLlink.h" -#include "util/GLutil.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLutil.h" #include "OnlineViewer_impl.h" #include "GLscene.h" diff --git a/util/viewer/OnlineViewer_impl.h b/util/viewer/OnlineViewer_impl.h index 68213450765..458f8d6592e 100644 --- a/util/viewer/OnlineViewer_impl.h +++ b/util/viewer/OnlineViewer_impl.h @@ -1,7 +1,7 @@ #include #include #include -#include "util/LogManager.h" +#include "hrpsys/util/LogManager.h" class GLscene; class GLbody; diff --git a/util/viewer/main.cpp b/util/viewer/main.cpp index 2e9f293de03..ba82c42a126 100644 --- a/util/viewer/main.cpp +++ b/util/viewer/main.cpp @@ -7,10 +7,10 @@ #endif #include #include -#include "util/GLlink.h" -#include "util/GLbody.h" -#include "util/GLutil.h" -#include "util/SDLUtil.h" +#include "hrpsys/util/GLlink.h" +#include "hrpsys/util/GLbody.h" +#include "hrpsys/util/GLutil.h" +#include "hrpsys/util/SDLUtil.h" #include "OnlineViewer_impl.h" #include "GLscene.h" @@ -37,7 +37,7 @@ int main(int argc, char *argv[]) int wsize=0; double maxEdgeLen=0.0; bool useDefaultLights=true; - float bgColor[3]; + float bgColor[3]={0,0,0}; for (int i=1; i
    implementation_idNullComponent
    categoryexample