Compare revisions

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

Source

Target

Show changes

Only incoming changes from source

Include changes to target after source was created

Commits on Source 137

69177f44 revert part of commit cc57de80 to fix bug
Oct 20, 2017
345ff1b2 [ros] Add dependence to pal_hardwarde_interfaces.
Dec 21, 2017
bda1e849 Make sure that the robot is not falling when switching to effort control mode.
Jan 4, 2018
79121796 Read the current position as the desired one to wait for the effort control.
Jan 8, 2018
29ebd62c Remove debugger.
Jan 8, 2018
caacdcd2 [roscontrol_sot] Add information when an exception is thrown.
Jan 13, 2018
eb70ee89 gitlab-ci
Feb 15, 2018
3d172a97 [gitlab-ci] clone from local repo
Feb 15, 2018
b07aca89 [CI] generated .gitlab-ci.yml
Feb 15, 2018
378a3086 [CI] pull robotpkg
Feb 15, 2018
76950ea2 Change package.xml version and fix a problem in roscontrol-sot-controller.cpp
Feb 15, 2018
d5c48e33 Fix exception and string access.
Feb 15, 2018
608548bd [gitlab] Remove make test.
Feb 15, 2018
d7329bdc Change version to 0.0.6
Feb 16, 2018
1baceda1 [cmake] Makes CMakeLists.txt independent from the branch.
Feb 26, 2018
1a273a9b Makes the code compiling both for real robot and simulation environment.
Feb 26, 2018
44200474 [ros] Add specific xml file for gazebo.
Feb 26, 2018
a46810c4 Fix link to proper plugin reference.
Feb 26, 2018
486a29f5 Fix roscontrol for topic/pyrene.
Feb 26, 2018
5af60cff Fix comments on talos_roscontrol_sot_talos/launch/sot_talos_controller_gazebo_effort.launch
Feb 26, 2018
c2bfd5ae Update PIDs values for the real robot
Feb 26, 2018
e772daef Add sot_pyrene_bringup in the metapkg talos.
Mar 7, 2018
ec037976 Corrected CMakeList.txt
Mar 7, 2018
5d1dd914 Fix state_vector_map
Mar 7, 2018
db50a20a Fix pyrene.yaml and integrates parallel joints (grippers)
Mar 8, 2018
7bf08858 Fix parameters based on robot feedback.
Mar 9, 2018
633ebd90 Add dubnium
Mar 13, 2018
30f46451 Fix sot_talos_params.yaml (use position control)
Apr 5, 2018
8e2c4a32 Update version number in package.xml
Apr 10, 2018
e430bcc7 Add duration log file to know the computation variation.
Apr 11, 2018
f75ef1ab Specify time dt inside roscontrol-sot for subsampling.
Apr 11, 2018
3e095f1a Update version number in package.xml
Apr 11, 2018
3ee11b3a Add duration log file to know the computation variation.
Apr 11, 2018
63089b9f Specify time dt inside roscontrol-sot for subsampling.
Apr 11, 2018
75d1e8de Initial commit
Apr 17, 2018
664cff91 Improve the computation of the initial time.
Apr 17, 2018
1db67142 Update .gitignore
Apr 17, 2018
68e13460 [gitlab-ci] Update from rainboard.
Apr 20, 2018
9144cdb1 [CI] no tests no doc
Apr 20, 2018
0e1cbc1e Add ability to give a launch prefix in geometric_simu
Apr 24, 2018
2d960ac1 Add ROS param /sot/dt to geometric simu
Apr 24, 2018
ee5f5eb6 Removing namespace with talos_ prefix.
May 6, 2018
4a26911e Remove talos_roscontrol_sot.
May 7, 2018
ab6fa45f Add republish.py
May 7, 2018
d811b78d [talos_roscontrol_sot] Fix CMakeLists.txt and package.xml
May 7, 2018
ff422503 Merge branch 'topic/pyrene'
May 7, 2018
056d63bc Changed name talos_roscontrol_sot_talos in roscontrol_sot_talos
May 7, 2018
3ecfadef Fix gitignore
May 7, 2018
a62434ec [gitlab] Update the registery name.
May 7, 2018
7d242caf Adds LICENSE file.
May 7, 2018
c2ddac0a Change reference to controller.
May 7, 2018
9aeb53c0 [roscontrol_sot_talos] Fix talos_ prefix
May 7, 2018
eab37fc3 [roscontrol_sot_talos] Remove talos_ prefix.
May 7, 2018
09ce35ae Update package.xml
May 7, 2018
ae795996 [talos_metapkg_ros_control_sot] Fix package.xml
May 7, 2018
a3f86023 [roscontrol_sot_talos] Fix talos_ suffix for sot_talos_controller.launch
May 8, 2018
f3d1720c Add README.md
May 8, 2018
55b445d8 Move ROS param /sot/dt to /sot_controller/dt
May 9, 2018
1f0fc2ce Merge branch 'master' into 'master'
May 9, 2018
31a23ce5 Move ROS param /sot/dt to /sot_controller/dt
May 9, 2018
926f7b59 [sot_pyrene_bringup] Add dt information and prefix for rostopic publication.
May 9, 2018
e3818437 [sot_pyrene_bringup] dt.
May 9, 2018
c0e0a1c2 [gitlab] Add gitlab information.
May 9, 2018
7481dfcc Add README.md
May 9, 2018
80e9bc75 [roscontrol_sot_talos] Update package information.
May 9, 2018
8f8a2eb2 [sot-pyrene-bringup] update to 0.0.8 version.
May 9, 2018
1d43281b [roscontrol_sot_talos] Add republish capabilities to roscontrol_sot_talos.
May 9, 2018
d48fa159 Remove trailing whitespaces.
May 9, 2018
ead8c64f Remove trailing whitespace.
May 9, 2018
bc93f860 Fix trailing white spaces.
May 9, 2018
dc58b36f Fixing trailing white spaces.
May 9, 2018
782ad0ca Add geometric_simu_pyrene.launch
May 9, 2018
ebcb9a05 Fix trailing white spaces.
May 9, 2018
e01895b7 Remove trailing white spaces.
May 9, 2018
b860f595 Fix trailing whitespace.
May 9, 2018
602d8a1f [gitlab] Update gitlab-ci.
May 9, 2018
29d68ee7 Merge branch 'gh_master'
May 9, 2018
7377c26d Add ACCEPTED_LICENSE to robotpkg.conf
May 10, 2018
355e0847 [gitlab] Update .gitlab-ci
May 10, 2018
0aa77817 [gitlab-ci]
May 10, 2018
7bf58ef0 [gitlab-ci] Fix package name.
May 10, 2018
6e480db0 [gitlab] Add missing packages.
May 10, 2018
787b13af [gitlab-ci] Fix package name
May 10, 2018
6c332815 [gitlab] Fix indigo gitlab name packages.
May 10, 2018
6f677782 Reflects changes in roscontrol_sot to talos package.
May 15, 2018
e28973e9 Add launch file to display the SoT output in rviz (servo off mode)
May 15, 2018
72718b52 VERY IMPORTANT ! GOOD MAPPING FOR THE SOT ON PYRENE ! DO NOT CHANGE !
May 15, 2018
e28b3735 Makes republish reads /sot_controller/joint_names for the mapping
May 16, 2018
274183d1 Merge branch 'master' of gepgitlab.laas.fr:pyrene-dev/talos_metapkg_ros_control_sot
May 16, 2018
3a299285 Update to version 0.0.9
May 16, 2018
4c5f0495 Make display_sot.launch more flexible.
Jun 5, 2018
7c96f667 Fix gazebo simulation
Jun 5, 2018
d464dabc Add joint names to sot_pyrene_bringup/sot/pyrene.yaml
Jun 5, 2018
cd962bb5 Publish base_link position wrt odom in republish
Jun 20, 2018
a7e2fedf Improve republish
Jul 4, 2018
f8db9af3 In position control, send currents, accelerometer and gyrometer to SOT.
Jul 4, 2018
abdb87a0 Merge pull request #2 from jmirabel/master
Jul 6, 2018
f3465df9 For starting the effort part we switch to gains.
Jul 19, 2018
0f818e18 Remove integral terms in pid_efforts.
Jul 19, 2018
a9f0847c Fix sot_talos_params_effort.yaml for real robot.
Jul 19, 2018

37 additional commits have been omitted to prevent performance issues.

.git-blame-ignore-revs

0 → 100644

+2 −0

Original line number	Original line	Diff line number	Diff line
			# format (Guilhem Saurel, 2022-09-12)
			22bb5a0457fa244d13386673401469c00ad7a043

.gitignore

+32 −0

Original line number	Original line	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	Original line	Diff line number	Diff line
	variables:		include: https://rainboard.laas.fr/project/talos-metapkg-ros-control-sot/.gitlab-ci.yml
	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

.gitmodules

+0 −3

Original line number	Original line	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	Original line	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	Original line	Diff line number	Diff line
	BSD 2-Clause License		BSD 2-Clause License

	Copyright (c) 2017, Stack Of Tasks development team		Copyright (c) 2018, Stack Of Tasks development team
	All rights reserved.		All rights reserved.

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

README.md

0 → 100644

+40 −0

Original line number	Original line	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	Original line	Diff line number	Diff line
			[tool.black]
			exclude = "cmake"

roscontrol_sot_talos/CMakeLists.txt

0 → 100644

+47 −0

Original line number	Original line	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	Original line	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	Original line	Diff line number	Diff line
	sot_controller:		sot_controller:
	type: talos_sot_controller/RCSotController		type: sot_controller/RCSotController
	joints:		joints:
	- arm_left_1_joint		- arm_left_1_joint
	- arm_left_2_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	Original line	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,		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,		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_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
	head_1_joint, head_2_joint ]		]
	map_rc_to_sot_device: { motor-angles: motor-angles ,		map_rc_to_sot_device: { motor-angles: motor-angles ,
	joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,		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 }		torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
	effort_control_pd_motor_init:		position_control_init_pos:
	- name: leg_left_1_joint		- name: leg_left_1_joint
	p_gain: 100.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: leg_left_2_joint		- name: leg_left_2_joint
	p_gain: 120.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: leg_left_3_joint		- name: leg_left_3_joint
	p_gain: 100.0
	d_gain: 0.0
	des_pos: -0.411354		des_pos: -0.411354
	- name: leg_left_4_joint		- name: leg_left_4_joint
	p_gain: 130.0
	d_gain: 0.0
	des_pos: 0.859395		des_pos: 0.859395
	- name: leg_left_5_joint		- name: leg_left_5_joint
	p_gain: 75.0
	d_gain: 0.0
	des_pos: -0.448041		des_pos: -0.448041
	- name: leg_left_6_joint		- name: leg_left_6_joint
	p_gain: 75.0
	d_gain: 0.0
	des_pos: -0.001708		des_pos: -0.001708
	- name: leg_right_1_joint		- name: leg_right_1_joint
	p_gain: 100.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: leg_right_2_joint		- name: leg_right_2_joint
	p_gain: 120.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: leg_right_3_joint		- name: leg_right_3_joint
	p_gain: 100.0
	d_gain: 0.0
	des_pos: -0.411354		des_pos: -0.411354
	- name: leg_right_4_joint		- name: leg_right_4_joint
	p_gain: 130.0
	d_gain: 0.0
	des_pos: 0.859395		des_pos: 0.859395
	- name: leg_right_5_joint		- name: leg_right_5_joint
	p_gain: 75.0
	d_gain: 0.0
	des_pos: -0.448041		des_pos: -0.448041
	- name: leg_right_6_joint		- name: leg_right_6_joint
	p_gain: 75.0
	d_gain: 0.0
	des_pos: -0.001708		des_pos: -0.001708
	- name: torso_1_joint		- name: torso_1_joint
	p_gain: 30.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: torso_2_joint		- name: torso_2_joint
	p_gain: 30.0
	d_gain: 0.0
	des_pos: 0.006761		des_pos: 0.006761
	- name: arm_left_1_joint		- name: arm_left_1_joint
	p_gain: 20.0
	d_gain: 0.0
	des_pos: 0.25847		des_pos: 0.25847
	- name: arm_left_2_joint		- name: arm_left_2_joint
	p_gain: 100.0
	d_gain: 0.0
	des_pos: 0.173046		des_pos: 0.173046
	- name: arm_left_3_joint		- name: arm_left_3_joint
	p_gain: 50.0
	d_gain: 0.0
	des_pos: 0.0002		des_pos: 0.0002
	- name: arm_left_4_joint		- name: arm_left_4_joint
	p_gain: 50.0
	d_gain: 0.0
	des_pos: -0.525366		des_pos: -0.525366
	- name: arm_left_5_joint		- name: arm_left_5_joint
	p_gain: 50.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: arm_left_6_joint		- name: arm_left_6_joint
	p_gain: 30.0		des_pos: 0.0
	d_gain: 0.0
	des_pos: 0.1
	- name: arm_left_7_joint		- name: arm_left_7_joint
	p_gain: 30.0
	d_gain: 0.0
	des_pos: 0.1		des_pos: 0.1
	- name: gripper_left_joint		- name: gripper_left_joint
	p_gain: 0.002
	d_gain: 0.0
	des_pos: -0.005		des_pos: -0.005
	- name: arm_right_1_joint		- name: arm_right_1_joint
	p_gain: 20.0
	d_gain: 0.0
	des_pos: -0.25847		des_pos: -0.25847
	- name: arm_right_2_joint		- name: arm_right_2_joint
	p_gain: 100.0
	d_gain: 0.0
	des_pos: -0.173046		des_pos: -0.173046
	- name: arm_right_3_joint		- name: arm_right_3_joint
	p_gain: 50.0
	d_gain: 0.0
	des_pos: 0.0002		des_pos: 0.0002
	- name: arm_right_4_joint		- name: arm_right_4_joint
	p_gain: 50.0
	d_gain: 0.0
	des_pos: -0.525366		des_pos: -0.525366
	- name: arm_right_5_joint		- name: arm_right_5_joint
	p_gain: 50.0
	d_gain: 0.0
	des_pos: 0.0		des_pos: 0.0
	- name: arm_right_6_joint		- name: arm_right_6_joint
	p_gain: 30.0		des_pos: 0.0
	d_gain: 0.0
	des_pos: 0.1
	- name: arm_right_7_joint		- name: arm_right_7_joint
	p_gain: 30.0
	d_gain: 0.0
	des_pos: 0.1		des_pos: 0.1
	- name: gripper_right_joint		- name: gripper_right_joint
	p_gain: 0.002
	d_gain: 0.0
	des_pos: -0.005		des_pos: -0.005
	- name: head_1_joint		- name: head_1_joint
	p_gain: 1.0		des_pos: 0.0
	d_gain: 0.0
	des_pos:: 0.0
	- name: head_2_joint		- name: head_2_joint
	p_gain: 1.0		des_pos: 0.0
	d_gain: 0.0		dt: 0.001
	des_pos:: 0.0		subsampling: 2
	control_mode: EFFORT

roscontrol_sot_talos/config/sot_talos_params_control_mode.yaml

0 → 100644

+34 −0

Original line number	Original line	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	Original line	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	Original line	Diff line number	Diff line
	sot_controller:		sot_controller:
	libname: libsot-pyrene-controller.so		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,		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,		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,		torso_1_joint,torso_2_joint,
	@@ -9,5 +8,6 @@ sot_controller:
	head_1_joint, head_2_joint ]		head_1_joint, head_2_joint ]
	map_rc_to_sot_device: { motor-angles: motor-angles ,		map_rc_to_sot_device: { motor-angles: motor-angles ,
	joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,		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 }		torques: torques, control: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
	control_mode: EFFORT		dt: 0.001
			jitter: 0.0004

roscontrol_sot_talos/config/sot_talos_params_gazebo.yaml

0 → 100644

+80 −0

Original line number	Original line	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	Original line	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	Original line	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	Original line	Diff line number	Diff line
	<launch>		<launch>

	<!-- Sot Controller configuration -->		<!-- Sot Controller configuration -->
	<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params.yaml"/>		<rosparam command="load" file="$(find 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_control_mode.yaml"/>
			<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_controller.yaml" />

	<!-- Spawn walking controller -->		<!-- Spawn walking controller -->
	<node name="sot_controller_spawner"		<node name="sot_controller_spawner"
	@@ -10,4 +11,3 @@
	args="sot_controller" />		args="sot_controller" />

	</launch>		</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	Original line	Diff line number	Diff line
	<launch>		<launch>

	<!-- Sot Controller configuration -->		<!-- Sot Controller configuration -->
	<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_effort.yaml"/>		<rosparam command="load" file="$(find 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 roscontrol_sot_talos)/config/sot_talos_params_control_mode_effort.yaml"/>
	<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/pids.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 -->		<!-- Spawn walking controller -->
	<node name="sot_controller_spawner"		<node name="sot_controller_spawner"
	pkg="controller_manager" type="spawner" output="screen"		pkg="controller_manager" type="spawner" output="screen"
	args="sot_controller" />		args="sot_controller" />

	</launch>		</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	Original line	Diff line number	Diff line
	<launch>		<launch>

	<!-- Sot Controller configuration -->		<!-- Sot Controller configuration -->
	<rosparam command="load" file="$(find talos_roscontrol_sot_talos)/config/sot_talos_params_gazebo.yaml"/>		<rosparam command="load" file="$(find 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_controller.yaml" />
			<rosparam command="load" file="$(find roscontrol_sot_talos)/config/sot_talos_params_control_mode.yaml"/>

	<!-- Spawn walking controller -->		<!-- Spawn walking controller -->
	<node name="sot_controller_spawner"		<node name="sot_controller_spawner"
	pkg="talos_controller_manager" type="spawner" output="screen"		pkg="controller_manager" type="spawner" output="screen"
	args="sot_controller" />		args="sot_controller" />

	</launch>		</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	Original line	Diff line number	Diff line
	<launch>		<launch>

	<!-- Sot Controller configuration -->		<!-- 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" 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 -->		<!-- Spawn sot controller -->
	<node name="sot_controller_spawner"		<node name="sot_controller_spawner"
	pkg="talos_controller_manager" type="spawner" output="screen"		pkg="controller_manager" type="spawner" output="screen"
	args="sot_controller" />		args="sot_controller" />

	</launch>		</launch>

talos_roscontrol_sot_talos/package.xml→roscontrol_sot_talos/package.xml

+9 −5

Original line number	Original line	Diff line number	Diff line
	<?xml version="1.0"?>		<?xml version="1.0"?>
	<package>		<package>
	<name>talos_roscontrol_sot_talos</name>		<name>roscontrol_sot_talos</name>
	<version>0.0.6</version>		<version>0.3.1</version>
	<description>Wrapping Stack-of-tasks for Talos in ros-control</description>		<description>Wrapping Stack-of-tasks for Talos in ros-control</description>

	<!-- Maintainer email -->		<!-- Maintainer email -->
	@@ -41,7 +41,9 @@
	<build_depend>sensor_msgs</build_depend>		<build_depend>sensor_msgs</build_depend>
	<build_depend>realtime_tools</build_depend>		<build_depend>realtime_tools</build_depend>
	<build_depend>cmake_modules</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>roscpp</run_depend>
	<run_depend>control_msgs</run_depend>		<run_depend>control_msgs</run_depend>
	@@ -50,6 +52,8 @@
	<run_depend>rospy</run_depend>		<run_depend>rospy</run_depend>
	<run_depend>std_msgs</run_depend>		<run_depend>std_msgs</run_depend>
	<run_depend>cmake_modules</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 -->		<!-- The export tag contains other, unspecified, tags -->
	</package>		</package>

roscontrol_sot_talos/scripts/republish

0 → 100755

+79 −0

Original line number	Original line	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	Original line	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	Original line	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	Original line	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	Original line	Diff line number	Diff line
	@@ -5,35 +5,19 @@ project(sot_pyrene_bringup)
	find_package(catkin REQUIRED)		find_package(catkin REQUIRED)
	catkin_package()		catkin_package()

	#set(PROJECT_NAME sot_talos_bringup)		# set(PROJECT_NAME sot_talos_bringup) set(PROJECT_DESCRIPTION ROS package for
	#set(PROJECT_DESCRIPTION ROS package for Stack of Tasks on Talos)		# Stack of Tasks on Talos) set(PROJECT_URL "")
	#set(PROJECT_URL "")

	# setup_project()		# setup_project()

	INSTALL(		install(FILES DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/config)
	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}/
	)

	#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	Original line	Diff line number	Diff line
	@@ -6,6 +6,8 @@
	<arg name="robot" default="pyrene" />		<arg name="robot" default="pyrene" />
	<arg name="libsot" default="libsot-pyrene-controller.so" />		<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" >		<include file="$(find sot_pyrene_bringup)/launch/geometric_simu_context.launch" >
	<arg name="robot" value="$(arg robot)" />		<arg name="robot" value="$(arg robot)" />
	<arg name="libsot" value="$(arg libsot)" />		<arg name="libsot" value="$(arg libsot)" />
	@@ -17,6 +19,7 @@
	pkg="dynamic_graph_bridge"		pkg="dynamic_graph_bridge"
	type="geometric_simu"		type="geometric_simu"
	args=" --input-file $(arg libsot)"		args=" --input-file $(arg libsot)"
			launch-prefix="$(arg sot-launch-prefix)"
	respawn="true">		respawn="true">
	<param name="/sot/dg/geometric_simu" value="" />		<param name="/sot/dg/geometric_simu" value="" />
	</node>		</node>

sot_pyrene_bringup/launch/geometric_simu_context.launch

+5 −6

Original line number	Original line	Diff line number	Diff line
	@@ -10,7 +10,7 @@

	<!-- Load robot model. -->		<!-- Load robot model. -->
	<param name="robot_description"		<param name="robot_description"
	textfile="$(find talos_data)/urdf/talos_reduced.urdf" />		textfile="$(find talos_data)/urdf/pyrene.urdf" />


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

	<machine name="geometric_simu_machine"		<machine name="geometric_simu_machine"
	address="localhost">		address="localhost" />
	</machine >

	<!--		<!--
	Read joint_states topic and publish link positions to tf.		Read joint_states topic and publish link positions to tf.
	@@ -30,7 +29,7 @@
	-->		-->
	<node name="robot_state_publisher"		<node name="robot_state_publisher"
	pkg="robot_state_publisher"		pkg="robot_state_publisher"
	type="state_publisher"		type="robot_state_publisher"
	respawn="true">		respawn="true">
	<param name="tf_prefix" value="" />		<param name="tf_prefix" value="" />
	</node>		</node>

sot_pyrene_bringup/package.xml

+1 −1

Original line number	Original line	Diff line number	Diff line
	<package>		<package>
	<name>sot_pyrene_bringup</name>		<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>		<description>ROS package for Stack of Tasks on Pyrene</description>

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

sot_pyrene_bringup/sot/pyrene.yaml

+10 −2

Original line number	Original line	Diff line number	Diff line
	@@ -17,3 +17,11 @@ sot:
	gripper_right_motor_single_joint: -gripper_right_joint,		gripper_right_motor_single_joint: -gripper_right_joint,
	gripper_right_inner_single_joint: -gripper_right_joint,		gripper_right_inner_single_joint: -gripper_right_joint,
	gripper_right_fingertip_3_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	Original line	Diff line number	Diff line
	<package>		<package>
	<name>talos_metapkg_ros_control_sot</name>		<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>		<description>A set of packages that include sot-wrapper and talos sot params .</description>

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

	<license>BSD</license>		<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>		<author>Olivier Stasse</author>

	<buildtool_depend>catkin</buildtool_depend>		<buildtool_depend>catkin</buildtool_depend>

	<run_depend>talos_roscontrol_sot</run_depend>		<run_depend>roscontrol_sot</run_depend>
	<run_depend>talos_roscontrol_sot_talos</run_depend>		<run_depend>roscontrol_sot_talos</run_depend>
	<run_depend>realtime_tools</run_depend>		<run_depend>realtime_tools</run_depend>
	<run_depend>control_toolbox</run_depend>		<run_depend>control_toolbox</run_depend>

talos_roscontrol_sot/CMakeLists.txt

deleted100644 → 0

+0 −125

Original line number	Original line	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	Original line	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

Original line number	Original line	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	Original line	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	Original line	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	Original line	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	Original line	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	Original line	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	Original line	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	Original line	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	Original line	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(46length);
	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+fsID6+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	Original line	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	Original line	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	Original line	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	Original line	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	Original line	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

Source

Target

Files