Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • devel
  • pr/28
  • master
  • pal
  • topic/position-effort
  • v0.0.1
  • v0.0.10
  • v0.0.11
  • v0.0.12
  • v0.0.13
  • v0.0.2
  • v0.0.3
  • v0.0.4
  • v0.0.5
  • v0.0.6
  • v0.0.7
  • v0.0.8rc
  • v0.0.8rc1
  • v0.0.9
  • v0.1.0
  • v0.2.0
  • v0.3.0
  • v0.3.1
23 results

Target

Select target project
  • Guilhem Saurel / talos-metapkg-ros-control-sot
  • Stack Of Tasks / talos-metapkg-ros-control-sot
2 results
Select Git revision
  • devel
  • master
  • pal
  • topic/position-effort
  • v0.0.1
  • v0.0.10
  • v0.0.11
  • v0.0.12
  • v0.0.13
  • v0.0.2
  • v0.0.3
  • v0.0.4
  • v0.0.5
  • v0.0.6
  • v0.0.7
  • v0.0.8rc
  • v0.0.8rc1
  • v0.0.9
  • v0.1.0
  • v0.2.0
  • v0.3.0
  • v0.3.1
22 results
Show changes

Commits on Source 137

37 additional commits have been omitted to prevent performance issues.
50 files
+ 781
− 1951
Compare changes
  • Side-by-side
  • Inline

Files

.git-blame-ignore-revs

0 → 100644
+2 −0
Original line number Diff line number Diff line 
# format (Guilhem Saurel, 2022-09-12)

22bb5a0457fa244d13386673401469c00ad7a043

.gitignore

+32 −0
Original line number Diff line number Diff line 
*~

# 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

.gitlab-ci.yml

+1 −45
Original line number Diff line number Diff line 
variables:

  GIT_SSL_NO_VERIFY: "true"

  GIT_SUBMODULE_STRATEGY: "recursive"

  GIT_DEPTH: "3"

  NAMESPACE: pyrene-dev

  PROJECT: talos_metapkg_ros_control_sot

  IMAGE: "${CI_REGISTRY}/${NAMESPACE}/${PROJECT}"



cache:

  paths:

    - build/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

    - make checkout

    - make install

    - cd work.*/${PROJECT}*/

  except:

    - gh-pages





robotpkg-talos-metapkg-ros-control-sot-14.04:

  <<: *robotpkg-talos-metapkg-ros-control-sot

  image: ${IMAGE}:14.04



robotpkg-talos-metapkg-ros-control-sot-16.04:

  <<: *robotpkg-talos-metapkg-ros-control-sot

  image: ${IMAGE}:16.04

  

robotpkg-talos-metapkg-ros-control-sot-dubnium:

  <<: *robotpkg-talos-metapkg-ros-control-sot

  image: ${IMAGE}:dubnium
 
include: https://rainboard.laas.fr/project/talos-metapkg-ros-control-sot/.gitlab-ci.yml

.gitmodules

+0 −3
Original line number Diff line number Diff line 
[submodule "talos_roscontrol_sot/cmake"]

	path = talos_roscontrol_sot/cmake

	url = https://github.com/jrl-umi3218/jrl-cmakemodules.git

.pre-commit-config.yaml

0 → 100644
+38 −0
Original line number Diff line number Diff line 
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

talos_roscontrol_sot/LICENSE→LICENSE

+1 −1
Original line number Diff line number Diff line 
BSD 2-Clause License



Copyright (c) 2017, Stack Of Tasks development team

Copyright (c) 2018, Stack Of Tasks development team

All rights reserved.



Redistribution and use in source and binary forms, with or without

README.md

0 → 100644
+40 −0
Original line number Diff line number Diff line 
 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

``

pyproject.toml

0 → 100644
+2 −0
Original line number Diff line number Diff line 
[tool.black]

exclude = "cmake"

roscontrol_sot_talos/CMakeLists.txt

0 → 100644
+47 −0
Original line number Diff line number Diff line 
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})

roscontrol_sot_talos/config/pids_effort.yaml

0 → 100644
+54 −0
Original line number Diff line number Diff line 
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

talos_roscontrol_sot_talos/config/sot_talos_controller.yaml→roscontrol_sot_talos/config/sot_talos_controller.yaml

+1 −1
Original line number Diff line number Diff line 
sot_controller:

  type: talos_sot_controller/RCSotController

  type: sot_controller/RCSotController

  joints:

    - arm_left_1_joint

    - arm_left_2_joint

talos_roscontrol_sot_talos/config/sot_talos_params_effort.yaml→roscontrol_sot_talos/config/sot_talos_params.yaml

+11 −74
Original line number Diff line number Diff line
@@ -4,138 +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

    - 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
 
      des_pos: 0.0

  dt: 0.001

  subsampling: 2

roscontrol_sot_talos/config/sot_talos_params_control_mode.yaml

0 → 100644
+34 −0
Original line number Diff line number Diff line 
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}

roscontrol_sot_talos/config/sot_talos_params_control_mode_effort.yaml

0 → 100644
+34 −0
Original line number Diff line number Diff line 
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}

talos_roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml→roscontrol_sot_talos/config/sot_talos_params_effort.yaml

+3 −3
Original line number Diff line number Diff line 
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,5 +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 
  torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }

  dt: 0.001

  jitter: 0.0004

roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml

0 → 100644
+80 −0
Original line number Diff line number Diff line 
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

roscontrol_sot_talos/config/sot_talos_params_gazebo_effort.yaml

0 → 100644
+47 −0
Original line number Diff line number Diff line 
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

roscontrol_sot_talos/launch/display_sot.launch

0 → 100644
+24 −0
Original line number Diff line number Diff line 
<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>

talos_roscontrol_sot_talos/launch/sot_talos_controller.launch→roscontrol_sot_talos/launch/sot_talos_controller.launch

+13 −0
Original line number Diff line number Diff line 
<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"
@@ -10,4 +11,3 @@ 
        args="sot_controller" />



</launch>

talos_roscontrol_sot_talos/launch/sot_talos_controller_effort.launch→roscontrol_sot_talos/launch/sot_talos_controller_effort.launch

+14 −0
Original line number Diff line number Diff line 
<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" />

  <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" />



  <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" />

  <!-- Spawn walking controller -->

  <node name="sot_controller_spawner"

        pkg="controller_manager" type="spawner" output="screen"

        args="sot_controller" />



</launch>

talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch→roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch

+13 −0
Original line number Diff line number Diff line 
<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>

talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch→roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch

+14 −0
Original line number Diff line number Diff line 
<launch>



  <!-- Sot Controller configuration -->

  <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>

  <rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_gazebo_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 talos_hardware_gazebo)/config/pids.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_controller.yaml" />



  <!-- Spawn sot 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>

talos_roscontrol_sot_talos/package.xml→roscontrol_sot_talos/package.xml

+9 −5
Original line number Diff line number Diff line 
<?xml version="1.0"?>

<package>

  <name>talos_roscontrol_sot_talos</name>

  <version>0.0.6</version>

  <name>roscontrol_sot_talos</name>

  <version>0.3.1</version>

  <description>Wrapping Stack-of-tasks for Talos in ros-control</description>



  <!-- Maintainer email -->
@@ -41,7 +41,9 @@ 
  <build_depend>sensor_msgs</build_depend>

  <build_depend>realtime_tools</build_depend>

  <build_depend>cmake_modules</build_depend>

  <build_depend>talos_roscontrol_sot</build_depend>

  <build_depend>roscontrol_sot</build_depend>

  <build_depend>controller_interface</build_depend>

  <build_depend>sot-talos</build_depend>



  <run_depend>roscpp</run_depend>

  <run_depend>control_msgs</run_depend>
@@ -50,6 +52,8 @@ 
  <run_depend>rospy</run_depend>

  <run_depend>std_msgs</run_depend>

  <run_depend>cmake_modules</run_depend>

  <run_depend>talos_roscontrol_sot</run_depend>

  <run_depend>roscontrol_sot</run_depend>

  <run_depend>controller_interface</run_depend>

  <run_depend>sot-talos</run_depend>

  <!-- The export tag contains other, unspecified, tags -->

</package>

roscontrol_sot_talos/scripts/republish

0 → 100755
+79 −0
Original line number Diff line number Diff line 
#!/usr/bin/env python

# license removed for brevity

import rospy

import tf

from dynamic_graph_bridge_msgs.msg import Vector

from sensor_msgs.msg import JointState

import sys



# Arg 1

input_topic = "/robotState"

# Arg 2

output_topic = "/sot/joint_state"

# Arg 3

publish_root_wrt_odom = False

if len(sys.argv) > 1:

    input_topic = sys.argv[1]

    if len(sys.argv) > 2:

        output_topic = sys.argv[2]

        if len(sys.argv) > 3:

            publish_root_wrt_odom = sys.argv[3].lower() in ("true", "on")



rospy.init_node("sot_reemitter", anonymous=True)

pub = rospy.Publisher(output_topic, JointState, queue_size=10)



seqnb = 0

aJS = JointState()



if publish_root_wrt_odom:

    if output_topic.find("/") >= 0:

        output_prefix = output_topic.rsplit("/", 1)[0] + "/"

    else:

        output_prefix = ""

    rospy.loginfo(

        "Will publish "

        + output_prefix

        + "base_link with respect to "

        + output_prefix

        + "odom"

    )



jointnames = rospy.get_param("/sot_controller/joint_names")





def jointreceived(jstates):

    global seqnb

    seqnb = seqnb + 1

    time = rospy.Time.now()

    aJS.header.seq = seqnb

    aJS.header.stamp = time

    aJS.header.frame_id = "base_link"

    aJS.name = jointnames

    aJS.position = jstates.data[6:]

    aJS.velocity = []

    aJS.effort = []

    pub.publish(aJS)



    if publish_root_wrt_odom and len(jstates.data) > 6:

        br = tf.TransformBroadcaster()

        br.sendTransform(

            jstates.data[0:3],

            tf.transformations.quaternion_from_euler(

                jstates.data[3], jstates.data[4], jstates.data[5]

            ),

            time,

            output_prefix + "base_link",

            output_prefix + "odom",

        )





def listener():

    rospy.Subscriber(input_topic, Vector, jointreceived)

    rospy.spin()





if __name__ == "__main__":

    try:

        listener()

    except rospy.ROSInterruptException:

        pass

roscontrol_sot_talos/scripts/start_talos_gazebo.py

0 → 100755
+76 −0
Original line number Diff line number Diff line 
#!/usr/bin/env python

# O. Stasse 17/01/2020

# LAAS, CNRS



import rospy

import time

import roslaunch

import rospkg



from std_srvs.srv import Empty



# Start roscore

import subprocess



roscore = subprocess.Popen("roscore")

time.sleep(1)



# Get the path to talos_data

arospack = rospkg.RosPack()

talos_data_path = arospack.get_path("talos_data")



# Start talos_gazebo

rospy.init_node("starting_talos_gazebo", anonymous=True)

uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)

roslaunch.configure_logging(uuid)



cli_args = [

    talos_data_path + "/launch/talos_gazebo_alone.launch",

    "world:=empty_forced",

    "enable_leg_passive:=false",

]

roslaunch_args = cli_args[1:]

roslaunch_file = [

    (roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)

]



launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file)

launch_gazebo_alone.start()

rospy.loginfo("talos_gazebo_alone started")



rospy.wait_for_service("/gazebo/pause_physics")

gazebo_pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)

gazebo_pause_physics()



time.sleep(5)

# Spawn talos model in gazebo

launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(

    uuid, [talos_data_path + "/launch/talos_gazebo_spawn_hs.launch"]

)

# launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(

# uuid, [talos_data_path + "/launch/talos_gazebo_spawn_hs_wide.launch"]

# )

launch_gazebo_spawn_hs.start()

# rospy.loginfo("talos_gazebo_spawn_hs started")



rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")

time.sleep(5)

gazebo_unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)

gazebo_unpause_physics()



# Start roscontrol

launch_bringup = roslaunch.parent.ROSLaunchParent(

    uuid, [talos_data_path + "/launch/talos_bringup.launch"]

)

launch_bringup.start()

rospy.loginfo("talos_bringup started")



# Start sot

roscontrol_sot_talos_path = arospack.get_path("roscontrol_sot_talos")

launch_roscontrol_sot_talos = roslaunch.parent.ROSLaunchParent(

    uuid, [roscontrol_sot_talos_path + "/launch/sot_talos_controller_gazebo.launch"]

)

launch_roscontrol_sot_talos.start()

rospy.loginfo("roscontrol_sot_talos started")



rospy.spin()

roscontrol_sot_talos/scripts/start_talos_gazebo_16_04.py

0 → 100755
+65 −0
Original line number Diff line number Diff line 
#!/usr/bin/env python2.7

# O. Stasse 17/01/2020

# LAAS, CNRS



# This file is a temporary fix for the ubuntu version 16.04 of the script

# start_talos_gazebo.py.

# The path to talos_data cannot be retreive using the rospkg.RosPack() and

# get_path('talos_data') in 16.04.

# This should be investigated (see issue #4 "Hardcoded talos_data path for 16.04 in

# script start_talos_gazebo_16_04.py").



import rospy

import time

import roslaunch

from std_srvs.srv import Empty



# Start roscore

import subprocess



roscore = subprocess.Popen("roscore")

time.sleep(1)



# Start talos_gazebo

rospy.init_node("starting_talos_gazebo", anonymous=True)

uuid = roslaunch.rlutil.get_or_generate_uuid(None, False)

roslaunch.configure_logging(uuid)



launch_gazebo_alone = roslaunch.parent.ROSLaunchParent(

    uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_alone.launch"]

)

launch_gazebo_alone.start()

rospy.loginfo("talos_gazebo_alone started")



rospy.wait_for_service("/gazebo/pause_physics")

gazebo_pause_physics = rospy.ServiceProxy("/gazebo/pause_physics", Empty)

gazebo_pause_physics()



time.sleep(3)

# Spawn talos model in gazebo

launch_gazebo_spawn_hs = roslaunch.parent.ROSLaunchParent(

    uuid, ["/opt/openrobots/share/talos_data/launch/talos_gazebo_spawn_hs.launch"]

)

launch_gazebo_spawn_hs.start()

rospy.loginfo("talos_gazebo_spawn_hs started")



rospy.wait_for_service("/gains/arm_left_1_joint/set_parameters")

time.sleep(3)

gazebo_unpause_physics = rospy.ServiceProxy("/gazebo/unpause_physics", Empty)

gazebo_unpause_physics()



# Start roscontrol

launch_bringup = roslaunch.parent.ROSLaunchParent(

    uuid, ["/opt/openrobots/share/talos_data/launch/talos_bringup.launch"]

)

launch_bringup.start()

rospy.loginfo("talos_bringup started")



# # Start sot

# Start sot in another terminal with "roslaunch roscontrol_sot_talos

# sot_talos_controller_gazebo.launch".

# in order to have the logs saved. Otherwise the data are not correctly dumped when the

# process is killed.





rospy.spin()

setup.cfg

0 → 100644
+4 −0
Original line number Diff line number Diff line 
[flake8]

exclude = cmake

max-line-length = 88

extend-ignore = E203

sot_pyrene_bringup/CMakeLists.txt

+15 −31
Original line number Diff line number Diff line
@@ -5,35 +5,19 @@ project(sot_pyrene_bringup) 
find_package(catkin REQUIRED)

catkin_package()



#set(PROJECT_NAME sot_talos_bringup)

#set(PROJECT_DESCRIPTION ROS package for Stack of Tasks on Talos)

#set(PROJECT_URL "")

# set(PROJECT_NAME sot_talos_bringup) set(PROJECT_DESCRIPTION ROS package for

# Stack of Tasks on Talos) set(PROJECT_URL "")



# setup_project()



INSTALL(

  FILES 

  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config

)



INSTALL(

  FILES

  launch/geometric_simu.launch

  launch/geometric_simu_context.launch

  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch

)



INSTALL(

  FILES

  sot/pyrene.yaml

  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/sot

)



INSTALL(

  FILES

  package.xml

  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/

)

install(FILES DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)



#setup_project_finalize()

install(FILES launch/geometric_simu.launch launch/geometric_simu_context.launch

        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch)



install(FILES sot/pyrene.yaml

        DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/sot)



install(FILES package.xml DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/)



# setup_project_finalize()

sot_pyrene_bringup/launch/geometric_simu.launch

+8 −5
Original line number Diff line number Diff line
@@ -6,6 +6,8 @@ 
  <arg name="robot" default="pyrene" />

  <arg name="libsot" default="libsot-pyrene-controller.so" />



  <arg name="sot-launch-prefix" default="" />



  <include file="$(find sot_pyrene_bringup)/launch/geometric_simu_context.launch" >

    <arg name="robot" value="$(arg robot)" />

    <arg name="libsot" value="$(arg libsot)" />
@@ -17,6 +19,7 @@ 
	pkg="dynamic_graph_bridge"

	type="geometric_simu"

        args=" --input-file $(arg libsot)"

        launch-prefix="$(arg sot-launch-prefix)"

	respawn="true">

    <param name="/sot/dg/geometric_simu" value="" />

  </node>

sot_pyrene_bringup/launch/geometric_simu_context.launch

+5 −6
Original line number Diff line number Diff line
@@ -10,7 +10,7 @@ 


   <!-- Load robot model. -->

  <param name="robot_description"

	 textfile="$(find talos_data)/urdf/talos_reduced.urdf" />

	 textfile="$(find talos_data)/urdf/pyrene.urdf" />





   <!-- Load robot sot params. -->
@@ -18,8 +18,7 @@ 
	 file="$(find sot_pyrene_bringup)/sot/pyrene.yaml" />



  <machine name="geometric_simu_machine"

           address="localhost">

  </machine >

    address="localhost" />



  <!--

     Read joint_states topic and publish link positions to tf.
@@ -30,7 +29,7 @@ 
    -->

  <node name="robot_state_publisher"

	pkg="robot_state_publisher"

	type="state_publisher"

	type="robot_state_publisher"

	respawn="true">

    <param name="tf_prefix" value="" />

  </node>

sot_pyrene_bringup/package.xml

+1 −1
Original line number Diff line number Diff line 
<package>

  <name>sot_pyrene_bringup</name>

  <version>0.0.1</version>

  <version>0.3.1</version>

  <description>ROS package for Stack of Tasks on Pyrene</description>



  <maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>

sot_pyrene_bringup/sot/pyrene.yaml

+10 −2
Original line number Diff line number Diff line
@@ -17,3 +17,11 @@ sot: 
    gripper_right_motor_single_joint: -gripper_right_joint,

    gripper_right_inner_single_joint: -gripper_right_joint,

    gripper_right_fingertip_3_joint: -gripper_right_joint}

sot_controller:

  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

  ]

  dt: 0.001

talos_metapkg_ros_control_sot/package.xml

+4 −4
Original line number Diff line number Diff line 
<package>

  <name>talos_metapkg_ros_control_sot</name>

  <version>0.0.6</version>

  <version>0.3.1</version>

  <description>A set of packages that include sot-wrapper and talos sot params .</description>



  <maintainer email="olivier.stasse@laas.fr">Olivier Stasse</maintainer>



  <license>BSD</license>



  <url type="repository">https://github.com/stack-of-tasks/roscontrol_sot</url>

  <url type="repository">https://github.com/stack-of-tasks/talos_metapkg_roscontrol_sot</url>



  <author>Olivier Stasse</author>



  <buildtool_depend>catkin</buildtool_depend>



  <run_depend>talos_roscontrol_sot</run_depend>

  <run_depend>talos_roscontrol_sot_talos</run_depend>

  <run_depend>roscontrol_sot</run_depend>

  <run_depend>roscontrol_sot_talos</run_depend>

  <run_depend>realtime_tools</run_depend>

  <run_depend>control_toolbox</run_depend>

talos_roscontrol_sot/CMakeLists.txt

deleted100644 → 0
+0 −125
Original line number Diff line number Diff line 
# Copyright (C) 2017 LAAS-CNRS

#

# Author: Olivier Stasse

#

# This program is free software: you can redistribute it and/or modify

# it under the terms of the GNU General Public License as published by

# the Free Software Foundation, either version 3 of the License, or

# (at your option) any later version.

#

# This program is distributed in the hope that it will be useful,

# but WITHOUT ANY WARRANTY; without even the implied warranty of

# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

# GNU General Public License for more details.

#

# You should have received a copy of the GNU General Public License

# along with this program.  If not, see <http://www.gnu.org/licenses/>.

cmake_minimum_required(VERSION 2.8.3)



# Authorize warning error.

SET(CXX_DISABLE_WERROR)



include(cmake/base.cmake)

include(cmake/ros.cmake)

include(cmake/GNUInstallDirs.cmake)

include(cmake/python.cmake)



#project(talos_roscontrol_sot)



OPTION(REAL_ROBOT "Compiling this package for the real robot" TRUE)



find_package(PkgConfig REQUIRED)



add_required_dependency(bullet)



#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

if(${REAL_ROBOT})

  set(additional_catkin_required_components

    controller_interface

    pal_hardware_interfaces

    )

  

  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DREAL_ROBOT")



else(${REAL_ROBOT})

  set(additional_catkin_required_components

    talos_controller_interface

    talos_pal_hardware_interfaces

    )

endif(${REAL_ROBOT})

find_package(catkin REQUIRED COMPONENTS

  roscpp

  rospy

  std_msgs

  dynamic_graph_bridge

  control_msgs

  sensor_msgs

  realtime_tools

  ${additional_catkin_required_components}

)



## LAAS cmake submodule part

set(PROJECT_DESCRIPTION "Integration of the Stack of Tasks in roscontrol")

set(PROJECT_NAME talos_roscontrol_sot)

set(PROJECT_URL "ssh://git@redmine.laas.fr/laas/users/ostasse/pyrene-talos/metapkg_talos_roscontrol_sot.git")



set(CXX_DISABLE_WERROR False)



include_directories(include ${bullet_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})



link_directories(${bullet_LIBRARY_DIRS})



SETUP_PROJECT()



# This is necessary so that the pc file generated by catking is similar to the on

# done directly by jrl-cmake-modules

catkin_package(CATKIN_DEPENDS

roscpp realtime_tools message_runtime dynamic_graph_bridge

LIBRARIES rcsot_controller)



###########

## Build ##

###########



## Specify additional locations of header files

## Your package locations should be listed before other locations

# include_directories(include)

include_directories( 

  ${catkin_INCLUDE_DIRS}

  )



## Declare a C++ library

add_library(talos_rcsot_controller 

src/roscontrol-sot-controller.cpp

src/log.cpp

)



## Add cmake target dependencies of the executable

## same as for the library above

# add_dependencies(roscontrol_sot_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})



## Specify libraries to link a library or executable target against

target_link_libraries(talos_rcsot_controller

  ${catkin_LIBRARIES}

  ${bullet_LIBRARIES}

  )





## Mark executables and/or libraries for installation

install(TARGETS talos_rcsot_controller DESTINATION lib )





foreach(dir config launch)

  install(DIRECTORY ${dir}

    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

  )

endforeach()



SETUP_PROJECT_FINALIZE()

talos_roscontrol_sot/README.md

deleted100644 → 0
+0 −70
Original line number Diff line number Diff line 
# Introduction

This package encapsulates a SoT graph to control a robot in the ros-control framework.

The intent is to make it generic and adapted to any robot through rosparam.

As the Stack-Of-Taks is a whole-body controller it tries to connect to all the available

resources of the robot. 



# rosparam



## Namespace

All the parameters regarding the SoT inside ros-control are in the namespace

```

/sot_controller

```



## Setting the SoT dynamic library which contains the robot device.



Let us assume that you want to control a TALOS robot.

You should then write a YAML file than can be named:

```

sot_talos_param.yaml

```



Its SoT device entity is located inside the following dynamic library:

```

/opt/openrobots/lib/libsot-pyrene-controller.so

```

Then inside the file sot_talos_param.yaml

```

  libname: libsot-pyrene-controller.so

```



## Specifying the actuated state vector

To map the joints from the URDF model to the SoT actuated state vector, it is simply done by giving the ordered list of the joints name in the URDF model.

For instance:

```

     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,head_1_joint, head_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

  ] 

```



## Specifying the map between the ros control data and the sot device entity.



This ros-control plugin connect to the robot (or simulated robot) by requesting the available ressources such as:

1. motor-angles: Reading of the motor position

2. joint-angles: Reading of the joint position

3. velocities: Reading/estimation of the joint velocities

4. torques: Reading/estimation of the joint torques

5. cmd-position: Command for the joint positions

6. cmd-effort: Command for the joint torques



For instance the map for a robot which maps __cmd-position__ to __joints__ and __cmd-effort__ to __effort__ in its device will hae

have the following lines in its param file:

```

map_rc_to_sot_device: { motor-angles: motor-angles ,

  joint-angles: joint-angles, velocities: velocities,

  torques: torques, cmd-joints: joints, cmd-effort: effort }

```

## Specifying the control mode

Robots with __ros-control__ can be controlled either in position (POSITION) or in torque (EFFORT)

using the __control_mode__ variable:



```

     control_mode: EFFORT

```

cmake @ 38d4efa7

Compare 38d4efa7 to 38d4efa7
Original line number Diff line number Diff line 
Subproject commit 38d4efa72f8e3fe087eb5abc72efd51747b2271d

talos_roscontrol_sot/config/gazebo_roscontrol_sot_controller_plugins.xml

deleted100644 → 0
+0 −9
Original line number Diff line number Diff line 
<library path="lib/libtalos_rcsot_controller">



  <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="talos_controller_interface::ControllerBase">

    <description>

       The roscontrol SotController generates whole body motions for your robot (following the talos interface).

    </description>

  </class>

</library>

talos_roscontrol_sot/config/roscontrol_sot_controller_plugins.xml

deleted100644 → 0
+0 −9
Original line number Diff line number Diff line 
<library path="lib/libtalos_rcsot_controller">



  <class name="talos_sot_controller/RCSotController" type="talos_sot_controller::RCSotController" base_class_type="controller_interface::ControllerBase">

    <description>

       The roscontrol SotController generates whole body motions for your robot (following the talos interface).

    </description>

  </class>

</library>

talos_roscontrol_sot/config/rosdoc.yaml

deleted100644 → 0
+0 −5
Original line number Diff line number Diff line 
- builder: doxygen

  name: C++ API

  output_dir: c++

  file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'

  #exclude_patterns: '*/ARDroneLib/*'

talos_roscontrol_sot/doc/Doxyfile.extra.in

deleted100644 → 0
+0 −3
Original line number Diff line number Diff line 
INPUT                  = @PROJECT_SOURCE_DIR@/include \

			 @PROJECT_SOURCE_DIR@/doc

talos_roscontrol_sot/doc/manifest.yaml

deleted100644 → 0
+0 −22
Original line number Diff line number Diff line 
actions: []

authors: Olivier Stasse <ostasse@laas.fr>

brief: ''

bugtracker: ''

depends:

- controller_interface

- roscpp

- std_msgs

- sensor_msgs

- realtime_tools

- catkin

- cmake_modules

- rospy

- control_msgs

description: Wrapping the Stack-of-tasks framework in ros-control

license: BSD

maintainers: Olivier Stasse <ostasse@laas.fr>

msgs: []

package_type: package

repo_url: ''

srvs: []

url: ''

talos_roscontrol_sot/launch/sot_controller_gazebo.launch

deleted100644 → 0
+0 −13
Original line number Diff line number Diff line 
<launch>



  <!-- Sot Controller configuration -->

  <!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_params_gazebo.yaml"/> -->

  <!-- <rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_your_robot_controller.yaml" /> -->

  

  <!-- Spawn walking controller -->

  <node name="sot_controller_spawner"

        pkg="talos_controller_manager" type="spawner" output="screen"

        args="sot_controller" />



</launch>

talos_roscontrol_sot/package.xml

deleted100644 → 0
+0 −53
Original line number Diff line number Diff line 
<?xml version="1.0"?>

<package>

  <name>talos_roscontrol_sot</name>

  <version>0.0.6</version>

  <description>Wrapping the Stack-of-tasks framework in ros-control for talos</description>



  <!-- Maintainer email --> 

  <maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>





  <!-- One license tag required, multiple allowed, one license per tag -->

  <license>BSD</license>





  <!-- Url tags are optional, but mutiple are allowed, one per tag -->

  <!-- Optional attribute type can be: website, bugtracker, or repository -->

  <!-- Example: -->

  <!-- <url type="website">http://wiki.ros.org/roscontrol_sot</url> -->





  <!-- Author tag -->

 <author email="ostasse@laas.fr">Olivier Stasse</author>





  <!-- The *_depend tags are used to specify dependencies -->

  <buildtool_depend>catkin</buildtool_depend>

  <build_depend>roscpp</build_depend>

  <build_depend>rospy</build_depend>

  <build_depend>std_msgs</build_depend>

  <build_depend>control_msgs</build_depend>

  <build_depend>sensor_msgs</build_depend>

  <build_depend>realtime_tools</build_depend>

  <build_depend>controller_interface</build_depend>

  <build_depend>pal_hardware_interfaces</build_depend>

  <build_depend>cmake_modules</build_depend>

  <build_depend>dynamic_graph_bridge</build_depend>



  <run_depend>roscpp</run_depend>

  <run_depend>control_msgs</run_depend>

  <run_depend>sensor_msgs</run_depend>

  <run_depend>realtime_tools</run_depend>

  <run_depend>rospy</run_depend>

  <run_depend>std_msgs</run_depend>

  <run_depend>controller_interface</run_depend>

  <run_depend>pal_hardware_interfaces</run_depend>

  <run_depend>cmake_modules</run_depend>

  <run_depend>message_runtime</run_depend>

  <run_depend>dynamic_graph_bridge</run_depend>

  <!-- The export tag contains other, unspecified, tags -->

  <export>

    <controller_interface plugin="${prefix}/config/roscontrol_sot_controller_plugins.xml"/>

  </export>

</package>

talos_roscontrol_sot/src/log.cpp

deleted100644 → 0
+0 −166
Original line number Diff line number Diff line 
/* 

   Olivier Stasse CNRS,

   31/07/2012

   Object to log the low-level informations of a robot.

*/

#include "log.hh"

#include <sys/time.h>

#include <sstream>

#include <fstream>

#include <iostream>



using namespace std;

using namespace rc_sot_system;



DataToLog::DataToLog()

{

}



void DataToLog::init(unsigned int nbDofs,long int length)

{

  motor_angle.resize(nbDofs*length);

  joint_angle.resize(nbDofs*length);

  velocities.resize(nbDofs*length);

  torques.resize(nbDofs*length);

  motor_currents.resize(nbDofs*length);

  orientation.resize(4*length);

  accelerometer.resize(3*length);

  gyrometer.resize(3*length);

  force_sensors.resize(4*6*length);

  temperatures.resize(nbDofs*length);

  timestamp.resize(length);



  for(unsigned int i=0;i<nbDofs*length;i++)

    { motor_angle[i] = joint_angle[i] = 

	velocities[i] = 0.0;  }



}





Log::Log():  

  lref_(0),

  lrefts_(0)

{

}



void Log::init(unsigned int nbDofs, unsigned int length)

{

  lref_ =0;

  lrefts_=0;

  nbDofs_=nbDofs;

  length_=length;

  StoredData_.init(nbDofs,length);

  struct timeval current;

  gettimeofday(&current,0);



  timeorigin_ = (double)current.tv_sec + 0.000001 * ((double)current.tv_usec);



}



void Log::record(DataToLog &aDataToLog)

{

  if ( (aDataToLog.motor_angle.size()!=nbDofs_) ||

       (aDataToLog.velocities.size()!=nbDofs_))

    return;



  for(unsigned int JointID=0;JointID<nbDofs_;JointID++)

    {

      if (aDataToLog.motor_angle.size()>JointID)

	StoredData_.motor_angle[JointID+lref_]= aDataToLog.motor_angle[JointID];

      if (aDataToLog.joint_angle.size()>JointID)

	StoredData_.joint_angle[JointID+lref_]= aDataToLog.joint_angle[JointID];

      if (aDataToLog.velocities.size()>JointID)

	StoredData_.velocities[JointID+lref_]= aDataToLog.velocities[JointID];

      if (aDataToLog.torques.size()>JointID)

	StoredData_.torques[JointID+lref_]= aDataToLog.torques[JointID];

      if (aDataToLog.motor_currents.size()>JointID)

	StoredData_.motor_currents[JointID+lref_]= aDataToLog.motor_currents[JointID];

      if (aDataToLog.temperatures.size()>JointID)

	StoredData_.temperatures[JointID+lref_]= aDataToLog.temperatures[JointID];

    }

  for(unsigned int axis=0;axis<3;axis++)

    {

      StoredData_.accelerometer[lrefts_*3+axis] = aDataToLog.accelerometer[axis];

      StoredData_.gyrometer[lrefts_*3+axis] = aDataToLog.gyrometer[axis];

    }

  for(unsigned int fsID=0;fsID<4;fsID++)

    {

      for(unsigned int axis=0;axis<6;axis++)

	{

	  StoredData_.force_sensors[lrefts_*24+fsID*6+axis] = 

	    aDataToLog.force_sensors[fsID*6+axis];

	}

    }

  struct timeval current;

  gettimeofday(&current,0);



  StoredData_.timestamp[lrefts_] = 

    ((double)current.tv_sec + 0.000001 * (double)current.tv_usec) - timeorigin_;



  lref_ += nbDofs_;

  lrefts_ ++;

  if (lref_>=nbDofs_*length_)

    {

      lref_=0;

      lrefts_=0;

    }

}



void Log::save(std::string &fileName)

{

  std::string suffix("-mastate.log");

  saveVector(fileName,suffix,StoredData_.motor_angle, nbDofs_);

  suffix="-jastate.log";

  saveVector(fileName,suffix,StoredData_.joint_angle, nbDofs_);

  suffix = "-vstate.log";

  saveVector(fileName,suffix,StoredData_.velocities, nbDofs_);

  suffix = "-torques.log";

  saveVector(fileName,suffix,StoredData_.torques, nbDofs_);

  suffix = "-motor-currents.log";

  saveVector(fileName,suffix,StoredData_.motor_currents, nbDofs_);

  suffix = "-accelero.log";

  saveVector(fileName,suffix,StoredData_.accelerometer, 3);

  suffix = "-gyro.log";

  saveVector(fileName,suffix,StoredData_.gyrometer, 3);



  ostringstream oss;

  oss << "-forceSensors.log";

  suffix = oss.str();

  saveVector(fileName,suffix,StoredData_.force_sensors, 6);



  suffix = "-temperatures.log";

  saveVector(fileName,suffix,StoredData_.temperatures, nbDofs_);

  

}



void Log::saveVector(std::string &fileName,std::string &suffix,

		     std::vector<double> &avector,

		     unsigned int size)

{

  ostringstream oss;

  oss << fileName;

  oss << suffix.c_str();

  std::string actualFileName= oss.str();

  ofstream aof(actualFileName.c_str());

  if (aof.is_open())

    {

      for(unsigned long int i=0;i<length_;i++)

	{

	  // Save timestamp

	  aof << StoredData_.timestamp[i] << " " ;



	  // Compute and save dt

	  if (i==0)

	    aof << StoredData_.timestamp[i] - StoredData_.timestamp[length_-1] << " ";

	  else

	    aof << StoredData_.timestamp[i] - StoredData_.timestamp[i-1] << " ";



	  // Save all data

	  for(unsigned long int datumID=0;datumID<size;datumID++)

	    aof << avector[i*size+datumID] << " " ;



	  aof << std::endl;

	}

      aof.close();

    }

}

talos_roscontrol_sot/src/log.hh

deleted100644 → 0
+0 −85
Original line number Diff line number Diff line 
/* 

   Olivier Stasse CNRS,

   19/12/2016

   Object to control the low-level part of TALOS.

*/



#ifndef _RC_SOT_SYSTEM_LOG_H_

#define _RC_SOT_SYSTEM_LOG_H_



#include <vector>

#include <string>



namespace rc_sot_system {



  struct DataToLog

  {

    // Measured angle values at the motor side.

    std::vector<double> motor_angle;

    // Measured angle at the joint side.

    std::vector<double> joint_angle;

    // Measured or computed velocities.

    std::vector<double> velocities;

    // Measured torques.

    std::vector<double> torques;

    // Reconstructed orientation (from internal IMU).

    std::vector<double> orientation;

    // Measured linear acceleration

    std::vector<double> accelerometer;

    // Measured angular velocities

    std::vector<double> gyrometer;

    // Measured force sensors

    std::vector<double> force_sensors;

    // Measured motor currents

    std::vector<double> motor_currents;

    // Measured temperatures

    std::vector<double> temperatures;



    // Timestamp

    std::vector<double> timestamp;



    DataToLog();

    void init(unsigned int nbDofs, long int length);





  };



  class Log

  {

  private:

    // Actuated informations logged.

    unsigned int nbDofs_;

    // Number of iterations to be logged.

    unsigned int length_;



    // Current position in the circular buffer for angles.

    long unsigned int lref_;

    // Current position int the circular buffer for timestamp

    // lref_ = lrefts_ * nbDofs_

    long unsigned int lrefts_;



    // Circular buffer for all the data.

    DataToLog StoredData_;



    double timeorigin_;





    // Save one vector of information.

    void saveVector(std::string &filename, 

		    std::string &suffix,

		    std::vector<double> &avector,

		    unsigned int);



  public:

  

    Log();



    void init(unsigned int nbDofs, unsigned int length);

    void record(DataToLog &aDataToLog);



    void save(std::string &fileName);

  



  };

}

#endif /* _RC_SOT_SYSTEM_LOG_H_ */

talos_roscontrol_sot/src/roscontrol-sot-controller.cpp

deleted100644 → 0
+0 −824

File deleted.

Preview size limit exceeded, changes collapsed.

talos_roscontrol_sot/src/roscontrol-sot-controller.hh

deleted100644 → 0
+0 −280
Original line number Diff line number Diff line 
///////////////////////////////////////////////////////////////////////////////

// Copyright (C) 2016, CNRS

//

// 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.

//   * Neither the name of hiDOF, Inc. nor the names of its

//     contributors may be used to endorse or promote products derived from

//     this software without specific prior written permission.

//

// 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 OWNER 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.

//////////////////////////////////////////////////////////////////////////////



/*

 * Author: Olivier STASSE

 */



#ifndef RC_SOT_CONTROLLER_H

#define RC_SOT_CONTROLLER_H



#include <string>

#include <map>



#ifdef REAL_ROBOT

#include <controller_interface/controller.h>

#include <hardware_interface/joint_command_interface.h>

#include <hardware_interface/imu_sensor_interface.h>

#include <hardware_interface/force_torque_sensor_interface.h>

#include <pal_hardware_interfaces/actuator_temperature_interface.h>

#else

#include <talos_controller_interface/controller.h>

#include <talos_hardware_interface/joint_command_interface.h>

#include <talos_hardware_interface/imu_sensor_interface.h>

#include <talos_hardware_interface/force_torque_sensor_interface.h>

#include <talos_pal_hardware_interfaces/actuator_temperature_interface.h>

#endif



#include <dynamic_graph_bridge/sot_loader_basic.hh>

#include <ros/ros.h>

#include <control_toolbox/pid.h>



#include "log.hh"



namespace talos_sot_controller 

{

  enum SotControlMode { POSITION, EFFORT};



  class XmlrpcHelperException : public ros::Exception

  {

  public:

    XmlrpcHelperException(const std::string& what)

      : ros::Exception(what) {}

  };



  

  struct EffortControlPDMotorControlData

  {

    control_toolbox::Pid pid_controller;



    //double p_gain,d_gain,i_gain;

    double prev;

    double vel_prev;

    double des_pos;

    double integ_err;



    EffortControlPDMotorControlData();

    //    void read_from_xmlrpc_value(XmlRpc::XmlRpcValue &aXRV);

    void read_from_xmlrpc_value(const std::string &prefix);

  };



  /**

     This class encapsulates the Stack of Tasks inside the ros-control infra-structure.

     

   */

#ifdef REAL_ROBOT

  namespace lhi = hardware_interface;

  namespace lci = controller_interface;

#else

  namespace lhi = talos_hardware_interface;

  namespace lci = talos_controller_interface;

#endif

  

  class RCSotController : public lci::ControllerBase,

			       SotLoaderBasic

  {

    

  protected:

    /// Robot nb dofs.

    size_t nbDofs_;

    

    /// Data log.

    rc_sot_system::DataToLog DataOneIter_;

  private:

   

    /// @{ \name Ros-control related fields

    

    /// \brief Vector of joint handles.

    std::vector<lhi::JointHandle> joints_;

    std::vector<std::string> joints_name_;



    /// \brief Vector towards the IMU.

    std::vector<lhi::ImuSensorHandle> imu_sensor_;



    /// \brief Vector of 6D force sensor.

    std::vector<lhi::ForceTorqueSensorHandle> ft_sensors_;

    

    /// \brief Vector of temperature sensors for the actuators.

    std::vector<lhi::ActuatorTemperatureSensorHandle> 

    act_temp_sensors_;

    

    /// \brief Interface to the joints controlled in position.

    lhi::PositionJointInterface * pos_iface_;



    /// \brief Interface to the joints controlled in force.

    lhi::EffortJointInterface * effort_iface_;

    

    /// \brief Interface to the sensors (IMU).

    lhi::ImuSensorInterface* imu_iface_;



    /// \brief Interface to the sensors (Force).

    lhi::ForceTorqueSensorInterface* ft_iface_;

    

    /// \brief Interface to the actuator temperature sensor.

    lhi::ActuatorTemperatureSensorInterface  * act_temp_iface_;



    /// @}



    /// \brief Log

    rc_sot_system::Log RcSotLog;

    /// @}

    

    const std::string type_name_;



    /// \brief Adapt the interface to Gazebo simulation

    bool simulation_mode_;



    /// \brief The robot can controlled in effort or position mode (default).

    SotControlMode control_mode_;



   

    /// \brief Implement a PD controller for the robot when the dynamic graph

    /// is not on.

    std::map<std::string,EffortControlPDMotorControlData> effort_mode_pd_motors_;

 

    /// \brief Map from ros-control quantities to robot device

    /// ros-control quantities are for the sensors:

    /// * motor-angles

    /// * joint-angles

    /// * velocities

    /// * torques

    /// ros-control quantities for control are:

    /// * joints

    /// * torques

    std::map<std::string,std::string> mapFromRCToSotDevice_;



  public :



    RCSotController ();



    /// \brief Read the configuration files, 

    /// claims the request to the robot and initialize the Stack-Of-Tasks.

    bool initRequest (lhi::RobotHW * robot_hw, 

		      ros::NodeHandle &robot_nh,

		      ros::NodeHandle &controller_nh,

		      std::set<std::string> & claimed_resources);



    /// \brief Display claimed resources

    void displayClaimedResources(std::set<std::string> & claimed_resources);



    /// \brief Claims

    bool init();



    /// \brief Read the sensor values, calls the control graph, and apply the control.

    /// 

    void update(const ros::Time&, const ros::Duration& );

    /// \brief Starting by filling the sensors.

    void starting(const ros::Time&);

    /// \brief Stopping the control

    void stopping(const ros::Time&);

    /// \brief Display the kind of hardware interface that this controller is using.

    virtual std::string getHardwareInterfaceType() const;



  protected:

    /// Initialize the roscontrol interfaces

    bool initInterfaces(lhi::RobotHW * robot_hw,

			ros::NodeHandle &,

			ros::NodeHandle &,

			std::set<std::string> & claimed_resources);



    /// Initialize the hardware interface using the joints.

    bool initJoints();

    /// Initialize the hardware interface accessing the IMU.

    bool initIMU();

    /// Initialize the hardware interface accessing the force sensors.

    bool initForceSensors();

    /// Initialize the hardware interface accessing the temperature sensors.

    bool initTemperatureSensors();



    ///@{ \name Read the parameter server

    /// \brief Entry point

    bool readParams(ros::NodeHandle &robot_nh);



    /// \brief Creates the list of joint names.

    bool readParamsJointNames(ros::NodeHandle &robot_nh);



    /// \brief Set the SoT library name.

    bool readParamsSotLibName(ros::NodeHandle &robot_nh);



    /// \Brief Set the mapping between ros-control and the robot device

    /// For instance the yaml file should have a line with map_rc_to_sot_device:

    ///   map_rc_to_sot_device: [ ]

    bool readParamsFromRCToSotDevice(ros::NodeHandle &robot_nh);

    

    /// \brief Read the control mode.

    bool readParamsControlMode(ros::NodeHandle & robot_nh);



    /// \brief Read the PID information of the robot in effort mode.

    bool readParamsEffortControlPDMotorControlData(ros::NodeHandle &robot_nh);



    ///@}



    /// \brief Fill the SoT map structures

    void fillSensorsIn(std::string &title, std::vector<double> & data);



    /// \brief Get the information from the low level and calls fillSensorsIn.

    void fillJoints();

    

    /// In the map sensorsIn_ creates the key "name_IMUNb"

    /// and associate to this key the vector data.

    void setSensorsImu(std::string &name,

		       int IMUNb,

		       std::vector<double> &data);



    /// @{ \name Fill the sensors 

    /// Read the imus and set the interface to the SoT.

    void fillImu();

    /// Read the force sensors

    void fillForceSensors();

    /// Read the temperature sensors

    void fillTempSensors();

    /// Entry point for reading all the sensors .

    void fillSensors();

    ///@}

    



    /// Extract control values to send to the simulator.

    void readControl(std::map<std::string,dgs::ControlValues> &controlValues);



    /// Map of sensor readings

    std::map <std::string,dgs::SensorValues> sensorsIn_;



    /// Map of control values

    std::map<std::string,dgs::ControlValues> controlValues_;



    /// \brief Command send to motors

    /// Depending on control_mode it can be either

    /// position control or torque control.

    std::vector<double> command_;

    

    /// One iteration: read sensor, compute the control law, apply control.

    void one_iteration();



  };

}



#endif /* RC_SOT_CONTROLLER_H */

talos_roscontrol_sot_talos/CMakeLists.txt

deleted100644 → 0
+0 −80
Original line number Diff line number Diff line 
cmake_minimum_required(VERSION 2.8.3)

project(talos_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

  dynamic_graph_bridge

  control_msgs

  sensor_msgs

  realtime_tools

  talos_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}

  )





## Add cmake target dependencies of the executable

## same as for the library above

# add_dependencies(roscontrol_sot_talos_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})







#############

## Install ##

#############



# all install targets should use catkin DESTINATION variables

# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html



## Mark executable scripts (Python etc.) for installation

## in contrast to setup.py, you can choose the destination

# install(PROGRAMS

#   scripts/my_python_script

#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

# )



## Mark executables and/or libraries for installation

foreach(dir config launch)

  install(DIRECTORY ${dir}

    DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}

  )

endforeach()





#############

## Testing ##

#############



## Add gtest based cpp test target and link libraries

# catkin_add_gtest(${PROJECT_NAME}-test test/test_roscontrol_sot_talos.cpp)

# if(TARGET ${PROJECT_NAME}-test)

#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})

# endif()



## Add folders to be run by python nosetests

# catkin_add_nosetests(test)

talos_roscontrol_sot_talos/config/sot_talos_params.yaml

deleted100644 → 0
+0 −12
Original line number Diff line number Diff line 
sot_controller:

  libname: libsot-pyrene-controller.so

  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,head_1_joint, head_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

  ]

  map_rc_to_sot_device: { motor-angles: motor-angles ,

  joint-angles: joint-angles, velocities: velocities,

  torques: torques, cmd-positions: control, cmd-effort: control }

  control_mode: POSITION

talos_roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml

deleted100644 → 0
+0 −13
Original line number Diff line number Diff line 
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,

  torques: torques, cmd-joints: control, cmd-effort: control }

  control_mode: POSITION
 
 No newline at end of file