Commit e7563ad4 authored by Olivier Stasse's avatar Olivier Stasse

Initial commit

parents
BSD 2-Clause License
Copyright (c) 2018, Stack Of Tasks development team
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
This metapackage allows starting the stack of tasks for TIAGO.
## Introduction
Two methods to use the SoT with TIAGO 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_tiago sot_tiago_controller_gazebo.launch
To start the SoT in effort mode control:
roslaunch roscontrol_sot_tiago sot_tiago_controller_gazebo_effort.launch
### On the real robot
To start the SoT in position mode control:
``
roslaunch roscontrol_sot_tiago sot_tiago_controller.launch
``
To start the SoT in effort mode control:
``
roslaunch roscontrol_sot_tiago sot_tiago_controller_effort.launch
``
## Using geometric_simu
``
roslaunch sot_tiago_bringup geometric_simu.launch
``
cmake_minimum_required(VERSION 2.8.3)
project(roscontrol_sot_tiago)
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
)
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_tiago_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()
catkin_install_python(PROGRAMS scripts/republish
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_roscontrol_sot_tiago.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)
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_1_joint: &arm_1_gains {p: 20, d: 0, i: 0, i_clamp: 14, torque_clamp: 150}
arm_2_joint: &arm_2_gains {p: 100, d: 0, i: 0, i_clamp: 14, torque_clamp: 150}
arm_3_joint: &arm_3_gains {p: 50, d: 0, i: 0, i_clamp: 9, torque_clamp: 100}
arm_4_joint: &arm_4_gains {p: 50, d: 0, i: 0, i_clamp: 9, torque_clamp: 100}
arm_5_joint: &arm_5_gains {p: 50, d: 0, i: 0, i_clamp: 5, torque_clamp: 50}
arm_6_joint: &arm_6_gains {p: 30, d: 0, i: 0, i_clamp: 3, torque_clamp: 30}
arm_7_joint: &arm_7_gains {p: 30, d: 0, i: 0, i_clamp: 3, torque_clamp: 30}
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}
sot_controller:
type: sot_controller/RCSotController
joints:
- 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_joint
- head_1_joint
- head_2_joint
- torso_joint
left_ft_sensor: left_ankle_ft
right_ft_sensor: right_ankle_ft
base_imu_sensor: base_imu
check_mode: False
sot_controller:
libname: libsot-tiago-controller.so
joint_names: [ torso_joint,
arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_joint,
head_1_joint, head_2_joint ]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
position_control_init_pos:
- name: torso_joint
des_pos: 0.0
- name: arm_1_joint
des_pos: 0.25847
- name: arm_2_joint
des_pos: 0.173046
- name: arm_3_joint
des_pos: 0.0002
- name: arm_4_joint
des_pos: -0.525366
- name: arm_5_joint
des_pos: 0.0
- name: arm_6_joint
des_pos: 0.0
- name: arm_7_joint
des_pos: 0.1
- name: gripper_joint
des_pos: -0.005
- name: head_1_joint
des_pos: 0.0
- name: head_2_joint
des_pos: 0.0
control_mode: POSITION
dt: 0.001
jitter: 0.0004
sot_controller:
libname: libsot-tiago-controller.so
joint_names: [ torso_joint,
arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_joint,
head_1_joint, head_2_joint ]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
control_mode: EFFORT
dt: 0.001
jitter: 0.0004
sot_controller:
libname: libsot-tiago-controller.so
simulation_mode: gazebo
joint_names: [ torso_joint,
arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_joint,
head_1_joint, head_2_joint ]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
position_control_init_pos:
- name: torso_joint
des_pos: 0.0
- name: arm_1_joint
des_pos: 0.25847
- name: arm_2_joint
des_pos: 0.173046
- name: arm_3_joint
des_pos: 0.0002
- name: arm_4_joint
des_pos: -0.525366
- name: arm_5_joint
des_pos: 0.0
- name: arm_6_joint
des_pos: 0.0
- name: arm_7_joint
des_pos: 0.1
- name: gripper_joint
des_pos: -0.005
- name: gripper_joint
des_pos: -0.005
- name: head_1_joint
des_pos: 0.0
- name: head_2_joint
des_pos: 0.0
control_mode: POSITION
dt: 0.001
jitter: 0.0004
sot_controller:
libname: libsot-tiago-controller.so
simulation_mode: gazebo
joint_names: [ torso_1_joint,torso_2_joint,
arm_1_joint, arm_2_joint, arm_3_joint, arm_4_joint, arm_5_joint, arm_6_joint, arm_7_joint, gripper_joint,
head_1_joint, head_2_joint ]
map_rc_to_sot_device: { motor-angles: motor-angles ,
joint-angles: joint-angles, velocities: velocities, forces: forces, currents: currents,
torques: torques, cmd-joints: control, cmd-effort: control, accelerometer_0: accelerometer_0, gyrometer_0: gyrometer_0 }
control_mode: EFFORT
dt: 0.001
<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_tiago" 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>
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_params.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_controller.yaml" />
<!-- Spawn walking controller -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_params_effort.yaml"/>
<rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find roscontrol_sot_tiago)/config/pids_effort.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_controller.yaml" />
<!-- Spawn walking controller -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_params_gazebo.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_controller.yaml" />
<!-- Spawn walking controller -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
<launch>
<!-- Sot Controller configuration -->
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_params_gazebo_effort.yaml"/>
<rosparam command="load" ns="/sot_controller/effort_control_pd_motor_init" file="$(find tiago_hardware_gazebo)/config/pids.yaml"/>
<rosparam command="load" file="$(find roscontrol_sot_tiago)/config/sot_tiago_controller.yaml" />
<!-- Spawn sot controller -->
<node name="sot_controller_spawner"
pkg="controller_manager" type="spawner" output="screen"
args="sot_controller" />
</launch>
<?xml version="1.0"?>
<package>
<name>roscontrol_sot_tiago</name>
<version>0.0.1</version>
<description>Wrapping Stack-of-tasks for Tiago in ros-control</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_tiago</url> -->
<!-- Author tag -->
<author email="ostasse@laas.fr">Olivier Stasse</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<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>cmake_modules</build_depend>
<build_depend>roscontrol_sot</build_depend>
<build_depend>controller_interface</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>cmake_modules</run_depend>
<run_depend>roscontrol_sot</run_depend>
<run_depend>controller_interface</run_depend>
<!-- The export tag contains other, unspecified, tags -->
</package>
#!/usr/bin/env python
# license removed for brevity
import rospy, tf
from dynamic_graph_bridge_msgs.msg import Vector
from sensor_msgs.msg import JointState
import sys
# Arg 1
input_topic = "/sot_hpp/state"
# 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
cmake_minimum_required(VERSION 2.8.3)
project(tiago_metapkg_ros_control_sot)
find_package(catkin REQUIRED)
catkin_metapackage()
<package>
<name>tiago_metapkg_ros_control_sot</name>
<version>0.0.1</version>
<description>A set of packages that include sot-wrapper and tiago sot params .</description>
<maintainer email="olivier.stasse@laas.fr">Olivier Stasse</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/stack-of-tasks/tiago_metapkg_roscontrol_sot</url>
<author>Olivier Stasse</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>roscontrol_sot</run_depend>
<run_depend>roscontrol_sot_tiago</run_depend>
<run_depend>realtime_tools</run_depend>
<run_depend>control_toolbox</run_depend>
<export>
<metapackage/>
</export>
</package>
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment