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

Target

Select target project
  • gsaurel/talos-metapkg-ros-control-sot
  • stack-of-tasks/talos-metapkg-ros-control-sot
2 results
Show changes
Commits on Source (120)
20 additional commits have been omitted to prevent performance issues.
Hide whitespace changes
Inline Side-by-side
Showing
with 498 additions and 274 deletions
.git-blame-ignore-revs 0 → 100644
View file @ fc8df013
# format (Guilhem Saurel, 2022-09-12)
22bb5a0457fa244d13386673401469c00ad7a043
.gitlab-ci.yml
View file @ fc8df013
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
except:
- gh-pages
before_script:
- mkdir -p ccache
script:
- cd /root/robotpkg/${CATEGORY}/${ROBOTPKG}
- git pull
- make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
- make install
- cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
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
.pre-commit-config.yaml 0 → 100644
View file @ fc8df013
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
README.md
View file @ fc8df013
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:
......@@ -7,13 +12,13 @@ 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
## 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
......@@ -22,7 +27,7 @@ To start the SoT in effort mode control:
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
......@@ -33,6 +38,3 @@ roslaunch roscontrol_sot_talos sot_talos_controller_effort.launch
``
roslaunch sot_pyrene_bringup geometric_simu.launch
``
pyproject.toml 0 → 100644
View file @ fc8df013
[tool.black]
exclude = "cmake"
roscontrol_sot_talos/CMakeLists.txt
View file @ fc8df013
......@@ -6,19 +6,17 @@ 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
controller_interface
)
# 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})
......@@ -26,55 +24,24 @@ link_directories(${bullet_LIBRARY_DIRS})
catkin_package()
###########
## Build ##
###########
# ##############################################################################
# Build ##
# ##############################################################################
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${catkin_INCLUDE_DIRS}
)
# Specify additional locations of header files Your package locations should be
# listed before other locations include_directories(include)
include_directories(${catkin_INCLUDE_DIRS})
# ##############################################################################
# Install ##
# ##############################################################################
## 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
# Mark executables and/or libraries for installation
foreach(dir config launch)
install(DIRECTORY ${dir}
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
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)
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
View file @ fc8df013
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
roscontrol_sot_talos/config/sot_talos_params.yaml
View file @ fc8df013
......@@ -2,12 +2,77 @@ 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,
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
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-positions: control, cmd-effort: control }
control_mode: POSITION
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
subsampling: 2
roscontrol_sot_talos/config/sot_talos_params_control_mode.yaml 0 → 100644
View file @ fc8df013
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
View file @ fc8df013
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}
roscontrol_sot_talos/config/sot_talos_params_effort.yaml
View file @ fc8df013
......@@ -8,135 +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 }
effort_control_pd_motor_init:
- 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
- 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
- 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
- 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
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
View file @ fc8df013
......@@ -5,10 +5,76 @@ 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,
torques: torques, cmd-joints: control, cmd-effort: control }
control_mode: POSITION
dt: 0.001
\ No newline at end of file
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
View file @ fc8df013
......@@ -9,6 +9,39 @@ 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 }
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
View file @ fc8df013
<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>
roscontrol_sot_talos/launch/sot_talos_controller.launch
View file @ fc8df013
......@@ -2,12 +2,12 @@
<!-- Sot Controller configuration -->
<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>
roscontrol_sot_talos/launch/sot_talos_controller_effort.launch
View file @ fc8df013
......@@ -2,14 +2,13 @@
<!-- Sot Controller configuration -->
<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" />
<rosparam command="load" file="$(find 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" />
<!-- Spawn walking controller -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
roscontrol_sot_talos/launch/sot_talos_controller_gazebo.launch
View file @ fc8df013
......@@ -3,11 +3,11 @@
<!-- Sot Controller configuration -->
<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="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
View file @ fc8df013
......@@ -2,13 +2,13 @@
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_gazebo_effort.yaml"/>
<rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find hardware_gazebo)/config/pids.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 roscontrol_sot_talos)/config/sot_talos_controller.yaml" />
<!-- Spawn sot controller -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
roscontrol_sot_talos/package.xml
View file @ fc8df013
<?xml version="1.0"?>
<package>
<name>roscontrol_sot_talos</name>
<version>0.0.8</version>
<version>0.3.1</version>
<description>Wrapping Stack-of-tasks for Talos in ros-control</description>
<!-- Maintainer email -->
<!-- Maintainer email -->
<maintainer email="ostasse@laas.fr">Olivier Stasse</maintainer>
......@@ -43,6 +43,7 @@
<build_depend>cmake_modules</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>
......@@ -53,5 +54,6 @@
<run_depend>cmake_modules</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
View file @ fc8df013
#!/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