Compare revisions

olivier stasse · Olivier Stasse · Olivier Stasse · Olivier Stasse · Olivier Stasse · Olivier Stasse
--- a/.git-blame-ignore-revs
+++ b/.git-blame-ignore-revs
+# format (Guilhem Saurel, 2022-09-12)
+22bb5a0457fa244d13386673401469c00ad7a043
--- a/.gitignore
+++ b/.gitignore
 *~
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
-variables:
-  GIT_SUBMODULE_STRATEGY: "recursive"
-  GIT_DEPTH: "3"
-  NAMESPACE: pyrene-dev
-  PROJECT: talos_metapkg_ros_control_sot
-  REGISTRY: gepgitlab.laas.fr:4567
-  IMAGE: "${REGISTRY}/${NAMESPACE}/${PROJECT}"
-  CCACHE_BASEDIR: "${CI_PROJECT_DIR}"
-  CCACHE_DIR: "${CI_PROJECT_DIR}/ccache"
-
-cache:
-  paths:
-    - ccache
-
-
-.robotpkg-talos-metapkg-ros-control-sot: &robotpkg-talos-metapkg-ros-control-sot
-  variables:
-    ROBOTPKG: talos-metapkg-ros-control-sot
-    CATEGORY: wip
-  before_script:
-    - mkdir -p build/ccache
-    - cd build
-    - export CCACHE_BASEDIR=${PWD}
-    - export CCACHE_DIR=${PWD}/ccache
-  script:
-    - cd /root/robotpkg/${CATEGORY}/${ROBOTPKG}
-    - git pull
-    - echo "MASTER_REPOSITORY = git ${CI_PROJECT_DIR}" >> Makefile
-    - echo "REPOSITORY.talos-metapkg-ros-control-sot = git ${CI_PROJECT_DIR}" >> /opt/openrobots/etc/robotpkg.conf
-    - echo "ACCEPTABLE_LICENSES+=pal-license" >> /opt/openrobots/etc/robotpkg.conf
-    - make checkout
-    - make install
-    - cd work.*/${PROJECT}*/
-  except:
-    - gh-pages
-
-
-robotpkg-talos-metapkg-ros-control-sot-14.04:
-  <<: *robotpkg-talos-metapkg-ros-control-sot
-  before_script:
-    - mkdir -p build/ccache
-    - cd build
-    - export CCACHE_BASEDIR=${PWD}
-    - export CCACHE_DIR=${PWD}/ccache
-    - apt-get update    
-    - apt-get install ros-indigo-twist-mux ros-indigo-joy-teleop ros-indigo-moveit-ros-move-group  ros-indigo-humanoid-nav-msgs ros-indigo-four-wheel-steering-msgs ros-indigo-urdf-geometry-parser
-  image: ${IMAGE}:14.04
-
-
-robotpkg-talos-metapkg-ros-control-sot-16.04:
-  <<: *robotpkg-talos-metapkg-ros-control-sot
-  before_script:
-    - mkdir -p build/ccache
-    - cd build
-    - export CCACHE_BASEDIR=${PWD}
-    - export CCACHE_DIR=${PWD}/ccache
-    - apt-get update        
-    - apt-get install ros-kinetic-twist-mux ros-kinetic-joy-teleop ros-kinetic-moveit-ros-move-group  ros-kinetic-humanoid-nav-msgs ros-kinetic-four-wheel-steering-msgs ros-kinetic-urdf-geometry-parser  
-  image: ${IMAGE}:16.04
-  
-robotpkg-talos-metapkg-ros-control-sot-dubnium:
-  <<: *robotpkg-talos-metapkg-ros-control-sot
-  before_script:
-    - mkdir -p build/ccache
-    - cd build
-    - export CCACHE_BASEDIR=${PWD}
-    - export CCACHE_DIR=${PWD}/ccache
-    - apt-get update
-    - apt-get install ros-indigo-twist-mux ros-indigo-joy-teleop ros-indigo-moveit-ros-move-group  ros-indigo-humanoid-nav-msgs ros-indigo-four-wheel-steering-msgs ros-indigo-urdf-geometry-parser
-  image: ${IMAGE}:dubnium
+include: https://rainboard.laas.fr/project/talos-metapkg-ros-control-sot/.gitlab-ci.yml
--- a/.pre-commit-config.yaml
+++ b/.pre-commit-config.yaml
+ci:
+    autoupdate_branch: 'devel'
+repos:
+-   repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v14.0.6
+    hooks:
+    -   id: clang-format
+        args: [--style=Google]
+-   repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.3.0
+    hooks:
+    -   id: check-added-large-files
+    -   id: check-ast
+    -   id: check-executables-have-shebangs
+    -   id: check-json
+    -   id: check-merge-conflict
+    -   id: check-symlinks
+    -   id: check-toml
+    -   id: check-yaml
+    -   id: debug-statements
+    -   id: destroyed-symlinks
+    -   id: detect-private-key
+    -   id: end-of-file-fixer
+    -   id: fix-byte-order-marker
+    -   id: mixed-line-ending
+    -   id: trailing-whitespace
+-   repo: https://github.com/psf/black
+    rev: 22.8.0
+    hooks:
+    -   id: black
+-   repo: https://github.com/PyCQA/flake8
+    rev: 5.0.4
+    hooks:
+    -   id: flake8
+-   repo: https://github.com/cheshirekow/cmake-format-precommit
+    rev: v0.6.13
+    hooks:
+    - id: cmake-format
--- a/LICENSE
+++ b/LICENSE
+BSD 2-Clause License
+
+Copyright (c) 2018, Stack Of Tasks development team
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
--- a/README.md
+++ b/README.md
+ This metapackage allows starting the stack of tasks for TALOS.
+
+[![Pipeline status](https://gitlab.laas.fr/stack-of-tasks/talos-metapkg-ros-control-sot/badges/master/pipeline.svg)](https://gitlab.laas.fr/stack-of-tasks/talos-metapkg-ros-control-sot/commits/master)
+[![Coverage report](https://gitlab.laas.fr/stack-of-tasks/talos-metapkg-ros-control-sot/badges/master/coverage.svg?job=doc-coverage)](https://gepettoweb.laas.fr/doc/stack-of-tasks/talos-metapkg-ros-control-sot/master/coverage/)
+[![Code style: black](https://img.shields.io/badge/code%20style-black-000000.svg)](https://github.com/psf/black)
+[![pre-commit.ci status](https://results.pre-commit.ci/badge/github/stack-of-tasks/talos-metapkg-ros-control-sot/master.svg)](https://results.pre-commit.ci/latest/github/stack-of-tasks/talos-metapkg-ros-control-sot)
+
+## Introduction
+
+Two methods to use the SoT with TALOS are possible:
+
+ 1.  One based on roscontrol_sot which is using the roscontrol interface. The SoT is then acting as a Controller object interacting with the hardware interface. This is was is used with Gazebo and on the real robot.
+ 2. One based on geometric-simu. This software is simply a taking the control provided by the SoT and integrates it using a Euler scheme. The order of integration is chosen automatically according to the control law.
+
+## Using roscontrol_sot
+
+### On Gazebo
+
+To start the SoT in position mode control:
+    roslaunch roscontrol_sot_talos sot_talos_controller_gazebo.launch
+
+To start the SoT in effort mode control:
+	roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
+
+### On the real robot
+
+To start the SoT in position mode control:
+``
+roslaunch roscontrol_sot_talos sot_talos_controller.launch
+``
+To start the SoT in effort mode control:
+``
+roslaunch roscontrol_sot_talos sot_talos_controller_effort.launch
+``
+
+## Using geometric_simu
+
+``
+roslaunch sot_pyrene_bringup geometric_simu.launch
+``
--- a/pyproject.toml
+++ b/pyproject.toml
+[tool.black]
+exclude = "cmake"
--- a/roscontrol_sot_talos/CMakeLists.txt
+++ b/roscontrol_sot_talos/CMakeLists.txt
+cmake_minimum_required(VERSION 2.8.3)
+project(roscontrol_sot_talos)
+
+find_package(PkgConfig REQUIRED)
+
+set(bullet_FOUND 0)
+pkg_check_modules(bullet REQUIRED bullet)
+
+# Find catkin macros and libraries if COMPONENTS list like find_package(catkin
+# REQUIRED COMPONENTS xyz) is used, also find other catkin packages
+find_package(
+  catkin REQUIRED
+  COMPONENTS roscpp
+             rospy
+             std_msgs
+             control_msgs
+             sensor_msgs
+             realtime_tools
+             controller_interface)
+
+include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
+
+link_directories(${bullet_LIBRARY_DIRS})
+
+catkin_package()
+
+# ##############################################################################
+# Build ##
+# ##############################################################################
+
+# Specify additional locations of header files Your package locations should be
+# listed before other locations include_directories(include)
+include_directories(${catkin_INCLUDE_DIRS})
+
+# ##############################################################################
+# Install ##
+# ##############################################################################
+
+# Mark executables and/or libraries for installation
+foreach(dir config launch)
+  install(DIRECTORY ${dir} DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+endforeach()
+
+catkin_install_python(
+  PROGRAMS scripts/republish scripts/start_talos_gazebo.py
+  scripts/start_talos_gazebo_16_04.py DESTINATION
+  ${CATKIN_PACKAGE_BIN_DESTINATION})
--- a/roscontrol_sot_talos/config/pids_effort.yaml
+++ b/roscontrol_sot_talos/config/pids_effort.yaml
+gains:
+  leg_left_1_joint:  &leg_1_gains {p:  100, d: 0, i:  0, i_clamp:   7, torque_clamp: 60}
+  leg_left_2_joint:  &leg_2_gains {p:  120, d: 0, i:  0, i_clamp:   14, torque_clamp: 160}
+  leg_left_3_joint:  &leg_3_gains {p:  100, d: 0, i:  0, i_clamp:   14, torque_clamp: 160}
+  leg_left_4_joint:  &leg_4_gains {p:  130, d: 0, i:  0, i_clamp:   25, torque_clamp: 300}
+  leg_left_5_joint:  &leg_5_gains {p:   75, d: 0, i:  0, i_clamp:   14, torque_clamp: 160}
+  leg_left_6_joint:  &leg_6_gains {p:   75, d: 0, i:  0, i_clamp:   9, torque_clamp: 100}
+  leg_right_1_joint: *leg_1_gains
+  leg_right_2_joint: *leg_2_gains
+  leg_right_3_joint: *leg_3_gains
+  leg_right_4_joint: *leg_4_gains
+  leg_right_5_joint: *leg_5_gains
+  leg_right_6_joint: *leg_6_gains
+
+
+  head_1_joint:                   {p: 1,  d: 0.0, i: 0, i_clamp: 5, torque_clamp: 8}
+  head_2_joint:                   {p: 1,  d: 0.0, i: 0, i_clamp: 1.5, torque_clamp: 8}
+
+  torso_1_joint:                  {p: 30, d: 0, i: 0, i_clamp: 10, torque_clamp: 100}
+  torso_2_joint:                  {p: 30, d: 0, i: 0, i_clamp: 10, torque_clamp: 100}
+
+  arm_right_1_joint: &arm_1_gains {p: 20, d: 0, i: 0,  i_clamp: 14, torque_clamp: 150}
+  arm_right_2_joint: &arm_2_gains {p: 100, d: 0, i: 0, i_clamp: 14, torque_clamp: 150}
+  arm_right_3_joint: &arm_3_gains {p: 50, d: 0, i: 0,  i_clamp: 9, torque_clamp: 100}
+  arm_right_4_joint: &arm_4_gains {p: 50, d: 0, i: 0,  i_clamp: 9, torque_clamp: 100}
+  arm_right_5_joint: &arm_5_gains {p: 50,  d: 0, i: 0, i_clamp: 5, torque_clamp: 50}
+  arm_right_6_joint: &arm_6_gains {p: 30,  d: 0, i: 0, i_clamp: 3, torque_clamp: 30}
+  arm_right_7_joint: &arm_7_gains {p: 30,  d: 0, i: 0, i_clamp: 3, torque_clamp: 30}
+  arm_left_1_joint:  *arm_1_gains
+  arm_left_2_joint:  *arm_2_gains
+  arm_left_3_joint:  *arm_3_gains
+  arm_left_4_joint:  *arm_4_gains
+  arm_left_5_joint:  *arm_5_gains
+  arm_left_6_joint:  *arm_6_gains
+  arm_left_7_joint:  *arm_7_gains
+
+  gripper_left_joint:  {p: 0.002, d: 0, i: 0, i_clamp: 10, torque_clamp: 100}
+  gripper_right_joint: {p: 0.002, d: 0, i: 0, i_clamp: 10, torque_clamp: 100}
+
+  #https://answers.ros.org/question/283537/how-to-do-mimic-joints-that-work-in-gazebo/
+  gripperleft_inner_double_joint: &gripper_gains {p: 20, d: 0.0, i: 0.0, i_clamp: 0.2, antiwindup: false}
+  gripper_left_fingertip_1_joint: *gripper_gains
+  gripper_left_fingertip_2_joint: *gripper_gains
+  gripper_left_motor_single_joint: *gripper_gains
+  gripper_left_fingertip_3_joint: *gripper_gains
+  gripper_left_inner_single_joint: *gripper_gains
+  gripper_left_inner_double_joint: *gripper_gains
+
+  gripper_right_inner_double_joint: *gripper_gains
+  gripper_right_fingertip_1_joint: *gripper_gains
+  gripper_right_fingertip_2_joint: *gripper_gains
+  gripper_right_motor_single_joint: *gripper_gains
+  gripper_right_fingertip_3_joint: *gripper_gains
+  gripper_right_inner_single_joint: *gripper_gains
--- a/talos_roscontrol_sot_talos/config/sot_talos_controller.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_controller.yaml
 sot_controller:
-  type: talos_sot_controller/RCSotController
+  type: sot_controller/RCSotController
  joints:
    - arm_left_1_joint
    - arm_left_2_joint

--- a/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml
@@ -4,139 +4,75 @@ sot_controller:
    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
    torso_1_joint,torso_2_joint,
    arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
-    arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint,
-    head_1_joint, head_2_joint ]
+    arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint, head_1_joint, head_2_joint
+  ]
  map_rc_to_sot_device: { motor-angles: motor-angles ,
  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
-  torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
-  effort_control_pd_motor_init:
+  torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
+  position_control_init_pos:
    - name: leg_left_1_joint
-      p_gain: 100.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: leg_left_2_joint
-      p_gain: 120.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: leg_left_3_joint
-      p_gain: 100.0
-      d_gain: 0.0
      des_pos: -0.411354
    - name: leg_left_4_joint
-      p_gain: 130.0
-      d_gain: 0.0
      des_pos: 0.859395
    - name: leg_left_5_joint
-      p_gain: 75.0
-      d_gain: 0.0
      des_pos: -0.448041
    - name: leg_left_6_joint
-      p_gain: 75.0
-      d_gain: 0.0
      des_pos: -0.001708
    - name: leg_right_1_joint
-      p_gain: 100.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: leg_right_2_joint
-      p_gain: 120.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: leg_right_3_joint
-      p_gain: 100.0
-      d_gain: 0.0
      des_pos: -0.411354
    - name: leg_right_4_joint
-      p_gain: 130.0
-      d_gain: 0.0
      des_pos: 0.859395
    - name: leg_right_5_joint
-      p_gain: 75.0
-      d_gain: 0.0
      des_pos: -0.448041
    - name: leg_right_6_joint
-      p_gain: 75.0
-      d_gain: 0.0
      des_pos: -0.001708
    - name: torso_1_joint
-      p_gain: 30.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: torso_2_joint
-      p_gain: 30.0
-      d_gain: 0.0
      des_pos: 0.006761
    - name: arm_left_1_joint
-      p_gain: 20.0
-      d_gain: 0.0
      des_pos: 0.25847
    - name: arm_left_2_joint
-      p_gain: 100.0
-      d_gain: 0.0
      des_pos: 0.173046
    - name: arm_left_3_joint
-      p_gain: 50.0
-      d_gain: 0.0
      des_pos: 0.0002
    - name: arm_left_4_joint
-      p_gain: 50.0
-      d_gain: 0.0
      des_pos: -0.525366
    - name: arm_left_5_joint
-      p_gain: 50.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: arm_left_6_joint
-      p_gain: 30.0
-      d_gain: 0.0
-      des_pos: 0.1
+      des_pos: 0.0
    - name: arm_left_7_joint
-      p_gain: 30.0
-      d_gain: 0.0
      des_pos: 0.1
    - name: gripper_left_joint
-      p_gain: 0.002
-      d_gain: 0.0
      des_pos: -0.005
    - name: arm_right_1_joint
-      p_gain: 20.0
-      d_gain: 0.0
      des_pos: -0.25847
    - name: arm_right_2_joint
-      p_gain: 100.0
-      d_gain: 0.0
      des_pos: -0.173046
    - name: arm_right_3_joint
-      p_gain: 50.0
-      d_gain: 0.0
      des_pos: 0.0002
    - name: arm_right_4_joint
-      p_gain: 50.0
-      d_gain: 0.0
      des_pos: -0.525366
    - name: arm_right_5_joint
-      p_gain: 50.0
-      d_gain: 0.0
      des_pos: 0.0
    - name: arm_right_6_joint
-      p_gain: 30.0
-      d_gain: 0.0
-      des_pos: 0.1
+      des_pos: 0.0
    - name: arm_right_7_joint
-      p_gain: 30.0
-      d_gain: 0.0
      des_pos: 0.1
    - name: gripper_right_joint
-      p_gain: 0.002
-      d_gain: 0.0
-      des_pos: -0.005 
+      des_pos: -0.005
    - name: head_1_joint
-      p_gain: 1.0
-      d_gain: 0.0
-      des_pos:: 0.0
+      des_pos: 0.0
    - name: head_2_joint
-      p_gain: 1.0
-      d_gain: 0.0
-      des_pos:: 0.0      
-  control_mode: EFFORT
-  dt: 0.001
\ No newline at end of file
+      des_pos: 0.0
+  dt: 0.001
+  subsampling: 2
--- a/roscontrol_sot_talos/config/sot_talos_params_control_mode.yaml
+++ b/roscontrol_sot_talos/config/sot_talos_params_control_mode.yaml
+sot_controller:
+  control_mode:
+    arm_left_1_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_left_2_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_left_3_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_left_4_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_left_5_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_left_6_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_left_7_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_1_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_2_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_3_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_4_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_5_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_6_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    arm_right_7_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    gripper_left_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    gripper_right_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    head_1_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    head_2_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    leg_left_1_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_left_2_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_left_3_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_left_4_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_left_5_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_left_6_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_right_1_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_right_2_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_right_3_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_right_4_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_right_5_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    leg_right_6_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    torso_1_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
+    torso_2_joint: { ros_control_mode: POSITION, sot_control_mode: VELOCITY}
--- a/roscontrol_sot_talos/config/sot_talos_params_control_mode_effort.yaml
+++ b/roscontrol_sot_talos/config/sot_talos_params_control_mode_effort.yaml
+sot_controller:
+  control_mode:
+    arm_left_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_7_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_7_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    gripper_left_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    gripper_right_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    head_1_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    head_2_joint: { ros_control_mode: POSITION, sot_control_mode: POSITION}
+    leg_left_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    torso_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    torso_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
--- a/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
+++ b/talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
 sot_controller:
  libname: libsot-pyrene-controller.so
-  simulation_mode: gazebo
  joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
    torso_1_joint,torso_2_joint,
@@ -9,6 +8,6 @@ sot_controller:
    head_1_joint, head_2_joint ]
  map_rc_to_sot_device: { motor-angles: motor-angles ,
  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
-  torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
-  control_mode: EFFORT
-  dt: 0.001  
+  torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
+  dt: 0.001
+  jitter: 0.0004
--- a/roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml
+++ b/roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml
+sot_controller:
+  libname: libsot-pyrene-controller.so
+  simulation_mode: gazebo
+  joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
+    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
+    torso_1_joint,torso_2_joint,
+    arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
+    arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint, head_1_joint, head_2_joint
+  ]
+  map_rc_to_sot_device: { motor-angles: motor-angles ,
+  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
+  torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
+  position_control_init_pos:
+    - name: leg_left_1_joint
+      des_pos: 0.0
+    - name: leg_left_2_joint
+      des_pos: 0.0
+    - name: leg_left_3_joint
+      des_pos: -0.411354
+    - name: leg_left_4_joint
+      des_pos: 0.859395
+    - name: leg_left_5_joint
+      des_pos: -0.448041
+    - name: leg_left_6_joint
+      des_pos: -0.001708
+    - name: leg_right_1_joint
+      des_pos: 0.0
+    - name: leg_right_2_joint
+      des_pos: 0.0
+    - name: leg_right_3_joint
+      des_pos: -0.411354
+    - name: leg_right_4_joint
+      des_pos: 0.859395
+    - name: leg_right_5_joint
+      des_pos: -0.448041
+    - name: leg_right_6_joint
+      des_pos: -0.001708
+    - name: torso_1_joint
+      des_pos: 0.0
+    - name: torso_2_joint
+      des_pos: 0.006761
+    - name: arm_left_1_joint
+      des_pos: 0.25847
+    - name: arm_left_2_joint
+      des_pos: 0.173046
+    - name: arm_left_3_joint
+      des_pos: 0.0002
+    - name: arm_left_4_joint
+      des_pos: -0.525366
+    - name: arm_left_5_joint
+      des_pos: 0.0
+    - name: arm_left_6_joint
+      des_pos: 0.0
+    - name: arm_left_7_joint
+      des_pos: 0.1
+    - name: gripper_left_joint
+      des_pos: -0.005
+    - name: arm_right_1_joint
+      des_pos: -0.25847
+    - name: arm_right_2_joint
+      des_pos: -0.173046
+    - name: arm_right_3_joint
+      des_pos: 0.0002
+    - name: arm_right_4_joint
+      des_pos: -0.525366
+    - name: arm_right_5_joint
+      des_pos: 0.0
+    - name: arm_right_6_joint
+      des_pos: 0.0
+    - name: arm_right_7_joint
+      des_pos: 0.1
+    - name: gripper_right_joint
+      des_pos: -0.005
+    - name: head_1_joint
+      des_pos: 0.0
+    - name: head_2_joint
+      des_pos: 0.0
+  dt: 0.001
+  jitter: 0.0004
+  verbosity_level: 5
--- a/roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
+++ b/roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml
+sot_controller:
+  libname: libsot-pyrene-controller.so
+  simulation_mode: gazebo
+  joint_names: [ leg_left_1_joint, leg_left_2_joint, leg_left_3_joint, leg_left_4_joint, leg_left_5_joint, leg_left_6_joint,
+    leg_right_1_joint, leg_right_2_joint, leg_right_3_joint, leg_right_4_joint, leg_right_5_joint, leg_right_6_joint,
+    torso_1_joint,torso_2_joint,
+    arm_left_1_joint, arm_left_2_joint, arm_left_3_joint, arm_left_4_joint, arm_left_5_joint, arm_left_6_joint, arm_left_7_joint, gripper_left_joint,
+    arm_right_1_joint, arm_right_2_joint, arm_right_3_joint, arm_right_4_joint, arm_right_5_joint, arm_right_6_joint, arm_right_7_joint, gripper_right_joint,
+    head_1_joint, head_2_joint ]
+  map_rc_to_sot_device: { motor-angles: motor-angles ,
+  joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
+  torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
+  control_mode:
+    leg_left_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_left_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    leg_right_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    torso_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    torso_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_left_7_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    gripper_left_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_3_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_4_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_5_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_6_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    arm_right_7_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    gripper_right_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    head_1_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+    head_2_joint: { ros_control_mode: EFFORT, sot_control_mode: EFFORT}
+
+  dt: 0.001
--- a/roscontrol_sot_talos/launch/display_sot.launch
+++ b/roscontrol_sot_talos/launch/display_sot.launch
+<launch>
+  <arg name="input_topic" default="/sot_hpp/state"/>
+  <arg name="output_prefix" default="/sot"/>
+  <arg name="rviz" default="false"/>
+  <arg name="publish_root" default="true"/>
+
+  <node pkg="roscontrol_sot_talos" name="$(anon republish)" type="republish" args="$(arg input_topic) $(arg output_prefix)/joint_state $(arg publish_root)"/>
+
+  <node pkg="tf" type="static_transform_publisher"
+    name="$(anon base_link_broadcaster)"
+    args="0 0 0 0 0 0 1 odom $(arg output_prefix)/odom 100"
+    if="$(arg publish_root)" />
+  <node pkg="tf" type="static_transform_publisher"
+    name="$(anon base_link_broadcaster)"
+    args="0 0 0 0 0 0 1 odom $(arg output_prefix)/base_link 100"
+    unless="$(arg publish_root)" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="$(anon rob_st_pub)" >
+    <remap from="/joint_states" to="$(arg output_prefix)/joint_state" />
+    <param name="tf_prefix" value="$(arg output_prefix)" />
+  </node>
+
+  <node if="$(arg rviz)" pkg="rviz" type="rviz" name="$(anon rviz)" />
+</launch>
--- a/talos_roscontrol_sot_talos/launch/sot_talos_controller.launch
+++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller.launch
 <launch>

  <!-- Sot Controller configuration -->
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params.yaml"/>
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
-  
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params.yaml"/>
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_control_mode.yaml"/>
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
+
  <!-- Spawn walking controller -->
  <node name="sot_controller_spawner"
        pkg="controller_manager" type="spawner" output="screen"
        args="sot_controller" />

 </launch>
-
--- a/talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch
+++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch
 <launch>

  <!-- Sot Controller configuration -->
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_effort.yaml"/>
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/pids.yaml" />
-  
-  <env name="PYTHONPATH" value="/opt/pal/dubnium/lib/python2.7/dist-packages:/opt/ros/indigo/lib/python2.7/dist-packages:/opt/openrobots/lib/python2.7/dist-packages:/opt/openrobots/lib/python2.7/site-packages" />
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_effort.yaml"/>
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_control_mode_effort.yaml"/>
+  <rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find roscontrol_sot_talos)/config/pids_effort.yaml"/>
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
+
  <!-- Spawn walking controller -->
  <node name="sot_controller_spawner"
        pkg="controller_manager" type="spawner" output="screen"
        args="sot_controller" />

 </launch>
-
--- a/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch
+++ b/talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch
 <launch>

  <!-- Sot Controller configuration -->
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo.yaml"/>
-  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
-  
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_gazebo.yaml"/>
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
+  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_control_mode.yaml"/>
+
  <!-- Spawn walking controller -->
  <node name="sot_controller_spawner"
-        pkg="talos_controller_manager" type="spawner" output="screen"
+        pkg="controller_manager" type="spawner" output="screen"
        args="sot_controller" />

 </launch>
-
No results found