diff --git a/.gitignore b/.gitignore index bedb89c..22e577b 100644 --- a/.gitignore +++ b/.gitignore @@ -9,5 +9,5 @@ src/lisp/ src/sot_pr2/ srv/lisp/ srv_gen/ - +*~ *.cfgc diff --git a/.travis.yml b/.travis.yml new file mode 100644 index 0000000..3c981a4 --- /dev/null +++ b/.travis.yml @@ -0,0 +1,14 @@ +language: cpp +compiler: +- gcc +matrix: +notifications: + email: + - francois.keith@gmail.com +branches: + only: + - master + - groovy +script: ./.travis/build +before_install: ./.travis/before_install +after_success: ./.travis/after_success diff --git a/.travis/after_success b/.travis/after_success new file mode 100755 index 0000000..d39a840 --- /dev/null +++ b/.travis/after_success @@ -0,0 +1,6 @@ +#!/bin/sh +set -ev + +git config --global user.name "Travis CI" +git config --global user.email "thomas.moulard+travis@gmail.com" + diff --git a/.travis/before_install b/.travis/before_install new file mode 100755 index 0000000..7803ca3 --- /dev/null +++ b/.travis/before_install @@ -0,0 +1,58 @@ +#!/bin/bash +set -ev + +export UBUNTU_CODENAME=`lsb_release -s -c` +export ROS_VERSION=hydro + +echo "Ubuntu codename: $UBUNTU_CODENAME" +echo "ROS version: $ROS_VERSION" + +if `test x$UBUNTU_CODENAME = x`; then + echo "failed to determine Ubuntu codename" + return 1 +fi +if `test x$UBUNTU_CODENAME = x`; then + echo "failed to determine ROS version" + return 1 +fi + +git submodule update --init --recursive + +sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $UBUNTU_CODENAME main\" > /etc/apt/sources.list.d/ros-latest.list" +wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + +sudo apt-get update -qq + +sudo apt-get install -qq \ + doxygen doxygen-latex \ + libboost-all-dev \ + libeigen3-dev \ + liblapack-dev \ + libblas-dev \ + gfortran \ + python-dev \ + python-sphinx \ + python-numpy \ + libbullet-dev + + +sudo apt-get install -qq \ + ros-$ROS_VERSION-ros-base \ + ros-$ROS_VERSION-ros-control \ + ros-$ROS_VERSION-pr2-mechanism \ + ros-$ROS_VERSION-pr2-controllers \ + ros-$ROS_VERSION-control-msgs \ + ros-$ROS_VERSION-tf \ + ros-$ROS_VERSION-resource-retriever \ + ros-$ROS_VERSION-urdf \ + ros-$ROS_VERSION-urdfdom-py \ + ros-$ROS_VERSION-common-msgs \ + ros-$ROS_VERSION-rosdoc-lite \ + ros-$ROS_VERSION-robot-state-publisher \ + ros-$ROS_VERSION-cmake-modules \ + ros-hydro-ros-control \ + ros-hydro-pr2-controllers-msgs + + +sudo rosdep init +rosdep update diff --git a/.travis/build b/.travis/build new file mode 100755 index 0000000..68c9e6d --- /dev/null +++ b/.travis/build @@ -0,0 +1,88 @@ +#!/bin/sh +set -ev + +# Directories. +root_dir=`pwd` +build_dir="$root_dir/_travis/build" +install_dir="$root_dir/_travis/install" + +# Shortcuts. +git_clone="git clone --quiet --recursive" + +# Source ROS setup file +. /opt/ros/*/setup.sh + +# Create layout. +rm -rf "$build_dir" "$install_dir" +mkdir -p "$build_dir" +mkdir -p "$install_dir" + +# Setup environment variables. +export LD_LIBRARY_PATH="$install_dir/lib:$LD_LIBRARY_PATH" +export LD_LIBRARY_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`:$LD_LIBRARY_PATH" +export PKG_CONFIG_PATH="$install_dir/lib/pkgconfig:$PKG_CONFIG_PATH" +export PKG_CONFIG_PATH="$install_dir/lib/`dpkg-architecture -qDEB_BUILD_MULTIARCH`/pkgconfig:$PKG_CONFIG_PATH" + +export ROS_PACKAGE_PATH="$root_dir:$ROS_PACKAGE_PATH" + +install_dependency() +{ + echo "--> Compiling $1" + mkdir -p "$build_dir/$1" + cd "$build_dir" + $git_clone "git://github.com/$1" "$1" + cd "$build_dir/$1" + cmake . -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" + make install +} + +install_ros_stack() +{ + echo "--> Compiling $1" + mkdir -p "$build_dir/$1" + cd "$build_dir" + if [ "$#" -eq "2" ]; then + $git_clone "git://github.com/$1" "$1" -b $2 + else + $git_clone "git://github.com/$1" "$1" + fi + export ROS_PACKAGE_PATH="$build_dir/$1:$ROS_PACKAGE_PATH" +} + +# Retrieve jrl-mathtools +install_dependency jrl-umi3218/jrl-mathtools +install_dependency jrl-umi3218/jrl-mal +install_dependency laas/abstract-robot-dynamics +install_dependency jrl-umi3218/jrl-dynamics +install_dependency stack-of-tasks/dynamic-graph +install_dependency stack-of-tasks/dynamic-graph-python +install_dependency stack-of-tasks/sot-core +install_dependency stack-of-tasks/sot-tools +install_dependency stack-of-tasks/sot-dynamic +install_dependency laas/jrl_dynamics_urdf +install_ros_stack stack-of-tasks/redundant_manipulator_control + +cd "$root_dir" +# build dependencies +rosdep install dynamic_graph_bridge_msgs +rosmake dynamic_graph_bridge_msgs + +rosdep install dynamic_graph_bridge +rosmake dynamic_graph_bridge +cd "$build_dir" +cd stack-of-tasks/redundant_manipulator_control/dynamic_graph_bridge/build +cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" +rosmake dynamic_graph_bridge +make install + + +# rosmake and install sot_pr2 +cd "$root_dir" +export ROS_PACKAGE_PATH="$root_dir:$ROS_PACKAGE_PATH" +rosdep install sot_pr2 +rosmake sot_pr2 +cd build +cmake .. -DCMAKE_INSTALL_PREFIX:STRING="$install_dir" +rosmake sot_pr2 +make install + diff --git a/CMakeLists.txt b/CMakeLists.txt index 5a70af3..3e3564e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,6 +3,10 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) include(cmake/base.cmake) include(cmake/python.cmake) +if(NOT DEFINED CMAKE_INSTALL_LIBDIR) + set(CMAKE_INSTALL_LIBDIR lib) +endif() +include(cmake/GNUInstallDirs.cmake) set(ROS_BUILD_TYPE RelWithDebInfo) @@ -17,10 +21,19 @@ ADD_REQUIRED_DEPENDENCY("jrl-mal") ADD_REQUIRED_DEPENDENCY("dynamic-graph") ADD_REQUIRED_DEPENDENCY("dynamic-graph-python") ADD_REQUIRED_DEPENDENCY("sot-core") +ADD_REQUIRED_DEPENDENCY("jrl-dynamics-urdf >= 2.0.1-14") # This is required by the Python prologue. ADD_REQUIRED_DEPENDENCY("sot-dynamic") + +# Handle rpath necessary to handle ROS multiplace packages +# libraries inclusion +SET(CMAKE_SKIP_BUILD_RPATH FALSE) +SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE) +SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}") +SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + # Python INCLUDE(cmake/python.cmake) FINDPYTHON() @@ -28,20 +41,33 @@ INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS}) LINK_DIRECTORIES(${PYTHON_LIBRARY_DIRS}) set(SOURCES + src/pr2_controller_plugin.cpp + src/pr2_threaded_sot_controller.cpp src/pr2_sot_controller.cpp src/pr2_device.cpp) set(HEADERS + include/sot_pr2/pr2_controller_plugin.h + include/sot_pr2/pr2_threaded_sot_controller.h include/sot_pr2/pr2_sot_controller.h include/sot_pr2/pr2_device.h) rosbuild_add_library(sot_pr2 ${HEADERS} ${SOURCES}) -target_link_libraries(sot_pr2 dynamic-graph) -target_link_libraries(sot_pr2 sot-core) -target_link_libraries(sot_pr2 dynamic-graph-python) PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "dynamic-graph") PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "sot-core") PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "dynamic-graph-python") +PKG_CONFIG_USE_DEPENDENCY(sot_pr2 "jrl-dynamics-urdf") +install(TARGETS sot_pr2 DESTINATION ${CMAKE_INSTALL_LIBDIR}) + +rosbuild_add_executable(sot_pr2_fake_controller ${HEADERS} src/fake_controller.cpp) +PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "dynamic-graph") +PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "sot-core") +PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "dynamic-graph-python") +PKG_CONFIG_USE_DEPENDENCY(sot_pr2_fake_controller "jrl-dynamics-urdf") +target_link_libraries(sot_pr2_fake_controller sot_pr2) +install(TARGETS sot_pr2_fake_controller DESTINATION ${CMAKE_INSTALL_LIBDIR}) ADD_SUBDIRECTORY(src) +ADD_SUBDIRECTORY(python) + diff --git a/cmake b/cmake index f17f838..370de4f 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit f17f8387018e6b7405e94740b350a82e901e4384 +Subproject commit 370de4f4f8ca6122c7ddc13583a7a9465ccc8ad6 diff --git a/controller_plugins.xml b/controller_plugins.xml index ffdfe39..a483098 100644 --- a/controller_plugins.xml +++ b/controller_plugins.xml @@ -1,5 +1,5 @@ diff --git a/include/sot_pr2/pr2_controller_plugin.h b/include/sot_pr2/pr2_controller_plugin.h new file mode 100644 index 0000000..7a72b0a --- /dev/null +++ b/include/sot_pr2/pr2_controller_plugin.h @@ -0,0 +1,78 @@ +#ifndef PR2_CONTROLLER_PLUGIN_H +#define PR2_CONTROLLER_PLUGIN_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace sot_pr2 { + +typedef boost::shared_ptr Pr2JointPtr; + +class Pr2ControllerPlugin : public pr2_controller_interface::Controller { +public: + explicit Pr2ControllerPlugin(); + virtual ~Pr2ControllerPlugin(); + + virtual bool init(pr2_mechanism_model::RobotState *robot, + ros::NodeHandle &n); + virtual void starting(); + virtual void update(); + virtual void stopping(); + +private: + void fillSensors(); + void readControl(); + +private: + // SoT Controller + Pr2ThreadedSotController sot_controller_; + SensorMap sensorsIn_; + ControlMap controlValues_; + + std::vector joint_encoder_; + std::vector joint_velocity_; + std::vector joint_control_; + std::vector error_raw; + std::vector error; + + + // Pr2 Controller + int loop_count_; + ros::Time last_time_; + std::vector joints_; + std::vector pids_; + pr2_mechanism_model::RobotState *robot_; + + // Gripper + actionlib::SimpleActionClient *r_gripper_client_; + actionlib::SimpleActionClient *l_gripper_client_; + double r_gripper_position; + double l_gripper_position; + + // ROS interface + //ros::NodeHandle node_; + boost::scoped_ptr< + realtime_tools::RealtimePublisher< + control_msgs::JointTrajectoryControllerState> > controller_state_publisher_; + + ros::Publisher cmd_vel_pub_; + + tf::TransformListener listener_; + + double timeFromStart_; + + int _iter; + double _mean; +}; + +} + +#endif diff --git a/include/sot_pr2/pr2_device.h b/include/sot_pr2/pr2_device.h index 0a9b915..b87245b 100644 --- a/include/sot_pr2/pr2_device.h +++ b/include/sot_pr2/pr2_device.h @@ -2,34 +2,46 @@ #define PR2_DEVICE_H #include -#include -//#include -#include -#include - +#include +#include namespace sot_pr2 { - typedef boost::shared_ptr UrdfJointPtr; - typedef boost::shared_ptr Pr2JointPtr; - typedef std::pair jointAssociation_t; - typedef std::map jointMap_t; + typedef std::map SensorMap; + typedef std::map ControlMap; class Pr2Device : public dynamicgraph::sot::Device { DYNAMIC_GRAPH_ENTITY_DECL(); + public: + static const double TIMESTEP_DEFAULT; + public: Pr2Device(const std::string &name); virtual ~Pr2Device(); - virtual bool init(); + void setSensors(SensorMap &sensorsIn); - virtual void setup(jointMap_t &jm); + void setupSetSensors(SensorMap &sensorsIn); + void nominalSetSensors(SensorMap &sensorsIn); + void cleanupSetSensors(SensorMap &sensorsIn); - virtual void control(jointMap_t &jm); + void getControl(ControlMap &controlOut); + void getFakeControl(ControlMap &controlOut); - private: - static const double TIMESTEP_DEFAULT; + protected: + void updateRobotState(const std::vector &anglesIn); + + protected: double timestep_; + dynamicgraph::Vector previous_state_; + dynamicgraph::Signal robotState_; + + private: + dynamicgraph::Vector mlRobotState; + dynamicgraph::sot::MatrixRotation pose; + std::vector baseff_; + + int loop_count_; }; } diff --git a/include/sot_pr2/pr2_sot_controller.h b/include/sot_pr2/pr2_sot_controller.h index c7227e5..99b332d 100644 --- a/include/sot_pr2/pr2_sot_controller.h +++ b/include/sot_pr2/pr2_sot_controller.h @@ -1,31 +1,41 @@ #ifndef PR2_SOT_CONTROLLER_H #define PR2_SOT_CONTROLLER_H -#include -#include #include #include +#include namespace sot_pr2 { -class Pr2SotController : public pr2_controller_interface::Controller +class Pr2SotController : public dynamicgraph::sot::AbstractSotExternalInterface { +public: + static const std::string LOG_PYTHON; + public: explicit Pr2SotController(); + virtual ~Pr2SotController(); - virtual bool init(pr2_mechanism_model::RobotState *robot, - ros::NodeHandle &n); - virtual void starting(); - virtual void update(); - virtual void stopping(); + void setupSetSensors(SensorMap &sensorsIn); + void nominalSetSensors(SensorMap &sensorsIn); + void cleanupSetSensors(SensorMap &sensorsIn); -private: - Pr2Device *device_; - jointMap_t joint_map_; - dynamicgraph::Interpreter interpreter_; + void getControl(ControlMap &controlOut); - static const std::string LOG_PYTHON; - void runPython(std::ostream& file, const std::string& command); + boost::shared_ptr interpreter_; + ros::NodeHandle node_; + +protected: + void updateRobotState(std::vector &anglesIn); + + void runPython(std::ostream &file, + const std::string &command, + dynamicgraph::Interpreter &interpreter); + + virtual void startupPython(); + +private: + Pr2Device device_; }; } diff --git a/include/sot_pr2/pr2_threaded_sot_controller.h b/include/sot_pr2/pr2_threaded_sot_controller.h new file mode 100644 index 0000000..ed99b2c --- /dev/null +++ b/include/sot_pr2/pr2_threaded_sot_controller.h @@ -0,0 +1,51 @@ +#ifndef PR2_SOT_CONTROLLER_H +#define PR2_SOT_CONTROLLER_H + +#include +#include +#include + +namespace sot_pr2 { + +class Pr2ThreadedSotController : public dynamicgraph::sot::AbstractSotExternalInterface +{ +public: + static const std::string LOG_PYTHON; + +public: + explicit Pr2ThreadedSotController(); + virtual ~Pr2ThreadedSotController(); + + void init(); + + void setupSetSensors(SensorMap &sensorsIn); + void nominalSetSensors(SensorMap &sensorsIn); + void cleanupSetSensors(SensorMap &sensorsIn); + + void getControl(ControlMap &controlOut); + + boost::shared_ptr interpreter_; + ros::NodeHandle node_; + +protected: + void runPython(std::ostream &file, + const std::string &command, + dynamicgraph::Interpreter &interpreter); + + virtual void startupPython(); + +private: + Pr2Device device_; + + SensorMap _holdIn; + ControlMap _holdOut; + +public: + Pr2Device *device() {return &device_;} + SensorMap &holdIn() {return _holdIn;} + ControlMap &holdOut() {return _holdOut;} +}; + +} + +#endif diff --git a/launch/display.launch b/launch/display.launch index 28eaf72..67f3edc 100644 --- a/launch/display.launch +++ b/launch/display.launch @@ -6,8 +6,8 @@ - - + + diff --git a/launch/gazebo.launch b/launch/gazebo.launch index 1b001ae..9ccb165 100644 --- a/launch/gazebo.launch +++ b/launch/gazebo.launch @@ -1,8 +1,16 @@ - - - + + + + + - + + + + + diff --git a/launch/pr2_empty_world.launch b/launch/pr2_empty_world.launch new file mode 100644 index 0000000..e306f76 --- /dev/null +++ b/launch/pr2_empty_world.launch @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/spawn_pr2.launch b/launch/spawn_pr2.launch new file mode 100644 index 0000000..d02f99e --- /dev/null +++ b/launch/spawn_pr2.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/spawn_pr2_controller.launch b/launch/spawn_pr2_controller.launch new file mode 100644 index 0000000..fac7a5f --- /dev/null +++ b/launch/spawn_pr2_controller.launch @@ -0,0 +1,4 @@ + + + + diff --git a/manifest.xml b/manifest.xml index faac240..089f837 100644 --- a/manifest.xml +++ b/manifest.xml @@ -17,20 +17,13 @@ - - - - - - - + + - - diff --git a/pr2_sot_controller.yaml b/pr2_sot_controller.yaml new file mode 100644 index 0000000..f8dd605 --- /dev/null +++ b/pr2_sot_controller.yaml @@ -0,0 +1,102 @@ +sot_pr2: + type: sot_pr2/Pr2SotControllerPlugin + joints: + - bl_caster_rotation_joint + - bl_caster_l_wheel_joint + - bl_caster_r_wheel_joint + - br_caster_rotation_joint + - br_caster_l_wheel_joint + - br_caster_r_wheel_joint + - fl_caster_rotation_joint + - fl_caster_l_wheel_joint + - fl_caster_r_wheel_joint + - fr_caster_rotation_joint + - fr_caster_l_wheel_joint + - fr_caster_r_wheel_joint + - torso_lift_joint + - head_pan_joint + - head_tilt_joint + - l_shoulder_pan_joint + - l_shoulder_lift_joint + - l_upper_arm_roll_joint + - l_elbow_flex_joint + - l_forearm_roll_joint + - l_wrist_flex_joint + - l_wrist_roll_joint + - l_gripper_l_finger_joint + - l_gripper_l_finger_tip_joint + - l_gripper_motor_slider_joint + - l_gripper_motor_screw_joint + - l_gripper_r_finger_joint + - l_gripper_r_finger_tip_joint + - l_gripper_joint + - laser_tilt_mount_joint + - r_shoulder_pan_joint + - r_shoulder_lift_joint + - r_upper_arm_roll_joint + - r_elbow_flex_joint + - r_forearm_roll_joint + - r_wrist_flex_joint + - r_wrist_roll_joint + - r_gripper_l_finger_joint + - r_gripper_l_finger_tip_joint + - r_gripper_motor_slider_joint + - r_gripper_motor_screw_joint + - r_gripper_r_finger_joint + - r_gripper_r_finger_tip_joint + - r_gripper_joint + - torso_lift_motor_screw_joint + + gains: + fr_caster_l_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + torso_lift_motor_screw_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + r_shoulder_lift_joint: {p: 800.0, d: 10.0} + fl_caster_rotation_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_gripper_motor_slider_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_gripper_motor_screw_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + bl_caster_l_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + r_wrist_flex_joint: {p: 400.0, d: 4.0} + br_caster_r_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_shoulder_pan_joint: {p: 800.0, d: 10.0} + r_wrist_roll_joint: {p: 400.0, d: 4.0} + br_caster_l_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + fl_caster_l_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + r_gripper_r_finger_tip_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_elbow_flex_joint: {p: 100.0, d: 3.0} + r_gripper_l_finger_tip_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_forearm_roll_joint: {p: 500.0, d: 6.0} + laser_tilt_mount_joint: {p: 0.0, d: 0.0} + r_shoulder_pan_joint: {p: 800.0, d: 10.0} + r_gripper_motor_slider_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + fr_caster_r_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_wrist_roll_joint: {p: 400.0, d: 4.0} + r_gripper_r_finger_joint: {p: 0.0, d: 0.0} + r_elbow_flex_joint: {p: 100.0, d: 3.0} + head_pan_joint: {p: 25.0, i: 12.0, d: 2.0, i_clamp: 0.5} + bl_caster_r_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + torso_lift_joint: {p: 10000000.0, d: 1000000.0} + r_gripper_joint: {p: 0.0, d: 0.0} + l_gripper_joint: {p: 0.0, d: 0.0} + bl_caster_rotation_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + fl_caster_r_wheel_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_shoulder_lift_joint: {p: 800.0, d: 10.0} + r_gripper_l_finger_joint: {p: 0.0, d: 0.0} + r_forearm_roll_joint: {p: 500.0, d: 6.0} + r_gripper_motor_screw_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_gripper_l_finger_tip_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + br_caster_rotation_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_gripper_r_finger_tip_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_gripper_l_finger_joint: {p: 0.0, d: 0.0} + r_upper_arm_roll_joint: {p: 200.0, d: 3.0} + fr_caster_rotation_joint: {p: 10.0, i: 0.0, d: 0.0, i_clamp: 0.0} + l_upper_arm_roll_joint: {p: 200.0, d: 3.0} + head_tilt_joint: {p: 49.6, i: 2.0, d: 1.6, i_clamp: 0.1} + l_gripper_r_finger_joint: {p: 0.0, d: 0.0} + l_wrist_flex_joint: {p: 400.0, d: 4.0} + + pid_parameters: + p: 10.0 + i: 0.0 + d: 0.0 + i_clamp: 0.0 + diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000..a470cb4 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,8 @@ +INCLUDE(../cmake/python.cmake) +FINDPYTHON() + +INSTALL( + FILES pr2_catch.py pr2_sot_demo.py pr2_tests.py + DESTINATION ${PYTHON_SITELIB}/dynamic_graph/tutorial +) + diff --git a/python/controller_test_2.py b/python/controller_test_2.py new file mode 100644 index 0000000..3ae41ff --- /dev/null +++ b/python/controller_test_2.py @@ -0,0 +1,46 @@ +from dynamic_graph import plug +from dynamic_graph.sot.pr2.pr2_tasks import * +from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver +from dynamic_graph.ros import * +from time import sleep +plug(robot.device.state, robot.dynamic.position) +ros = Ros(robot) +sleep(2) +solver = initialize(robot) +robot.dynamic.velocity.value = robot.dimension*(0.,) +robot.dynamic.acceleration.value = robot.dimension*(0.,) +robot.dynamic.ffposition.unplug() +robot.dynamic.ffvelocity.unplug() +robot.dynamic.ffacceleration.unplug() +robot.dynamic.setProperty('ComputeBackwardDynamics','true') +robot.dynamic.setProperty('ComputeAccelerationCoM','true') +robot.device.control.unplug() +plug(solver.sot.control,robot.device.control) + +dt = 0.001 +taskRH = Pr2RightHandTask(robot) +taskLH = Pr2LeftHandTask(robot) +taskJL = Pr2JointLimitsTask(robot,dt) +taskContact = Pr2ContactTask(robot) +taskFov = Pr2FoVTask(robot,dt) +taskBase = Pr2BaseTask(robot) +rgrip = Pr2RightGripper(robot) +lgrip = Pr2LeftGripper(robot) + +from dynamic_graph.sot.core.meta_tasks_kine import gotoNd +targetRH = (0.60,-0.2,0.8) +gotoNd(taskRH,targetRH,'111',(4.9,0.9,0.01,0.9)) +targetLH = (0.65,0.3,0.6) +gotoNd(taskLH,targetLH,'111',(4.9,0.9,0.01,0.9)) +taskFov.goto3D(targetRH) +targetBase = (0.2, -0.1, 0, 0, 0, 0.0) +gotoNd(taskBase,targetBase,'100011',(4.9,0.9,0.01,0.9)) + +rgrip.open() +lgrip.open() + +solver.push(taskRH.task) +solver.push(taskLH.task) +solver.push(rgrip.task) +solver.push(lgrip.task) +solver.push(taskBase.task) diff --git a/python/pr2_catch.py b/python/pr2_catch.py index 0c7ca49..b169bd7 100644 --- a/python/pr2_catch.py +++ b/python/pr2_catch.py @@ -1,8 +1,13 @@ +# 0. TRICK: import Dynamic as the first command to avoid the crash at the exit +from dynamic_graph.sot.dynamics import Dynamic + # 1. Instanciate a Pr2 # The URDF description of the robot must have # been loaded in robot_description parameter # on the Ros Parameter Server -from dynamic_graph.sot.pr2.robot import Pr2 +# 1. Init robot, ros binding, solver +from dynamic_graph.sot.pr2.pr2_tasks import * +from dynamic_graph.sot.pr2.robot import * from dynamic_graph.sot.core import RobotSimu from dynamic_graph import plug robot = Pr2('PR2', device=RobotSimu('PR2')) @@ -13,9 +18,8 @@ from dynamic_graph.ros import Ros ros = Ros(robot) -# 3. Create a solver -from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver -solver = Solver(robot) +# Use kine solver (with inequalities) +solver = initialize(robot) # 4. Define a position task for the right hand from dynamic_graph.sot.core.meta_tasks_kine import gotoNd, MetaTaskKine6d @@ -30,17 +34,8 @@ gain=(4.9,0.9,0.01,0.9) gotoNd(taskRH,targetR,selec,gain) -# 5. Add a contact constraint with the robot and the floor -contact = MetaTaskKine6d('contact',robot.dynamic,'contact','left-ankle') -contact.feature.frame('desired') -contact.feature.selec.value = '011100' -contact.gain.setConstant(10) -contact.keep() -locals()['contactBase'] = contact - # 6. Push tasks in the solver solver.push(taskRH.task) -solver.push(contactBase.task) # Main loop dt=3e-3 @@ -53,4 +48,5 @@ def inc(): runner.once() [go,stop,next,n]=loopShortcuts(runner) -print 'Type go to run the solver loop' \ No newline at end of file +print 'Type go to run the solver loop' + diff --git a/python/pr2_sot_demo.py b/python/pr2_sot_demo.py index 31b856e..25556f0 100644 --- a/python/pr2_sot_demo.py +++ b/python/pr2_sot_demo.py @@ -1,6 +1,24 @@ +# 0. TRICK: import Dynamic as the first command to avoid the crash at the exit +from dynamic_graph.sot.dynamics import Dynamic + # 1. Init robot, ros binding, solver from dynamic_graph.sot.pr2.pr2_tasks import * -[robot,ros,sot] = initPr2RosSimuProblem() +from dynamic_graph.sot.pr2.robot import * +from dynamic_graph.sot.core.robot_simu import RobotSimu +from dynamic_graph import plug + +# creates the robot. +robot = Pr2('PR2', device=RobotSimu('PR2')) +plug(robot.device.state, robot.dynamic.position) + +# publish to ros +from dynamic_graph.ros import * +ros = Ros(robot) + +# Use kine solver (with inequalities) +from dynamic_graph.sot.dyninv import SolverKine +solver = initialize(robot, SolverKine) + # 2. Main loop from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts @@ -16,34 +34,42 @@ def inc(): taskRH = Pr2RightHandTask(robot) taskLH = Pr2LeftHandTask(robot) taskJL = Pr2JointLimitsTask(robot,dt) -taskContact = Pr2ContactTask(robot) taskFov = Pr2FoVTask(robot,dt) taskBase = Pr2BaseTask(robot) +taskChest = Pr2ChestTask(robot) +(taskWeight, featureWeight) = Pr2Weight(robot) +initPostureTask(robot) # 4. Formulate problem from dynamic_graph.sot.core.meta_tasks_kine import gotoNd +# 4.0 chest +gotoNd(taskChest,(-0.05,0.0,1),'111',(4.9,0.9,0.01,0.9)) + # 4.1 Right hand -targetRH = (0.60,0.2,0.8) +targetRH = (0.60,-0.2,0.8) gotoNd(taskRH,targetRH,'111',(4.9,0.9,0.01,0.9)) # 4.2 Left hand -targetLH = (0.65,0.6,0.3) +targetLH = (0.65,0.2,0.8) gotoNd(taskLH,targetLH,'111',(4.9,0.9,0.01,0.9)) # 4.3 Look at the right hand target taskFov.goto3D(targetRH) # 4.4 Base position -targetBase = (0,0,0,0,0,-0.2) +targetBase = (0,0,0,0,0,0) gotoNd(taskBase,targetBase,'100011',(4.9,0.9,0.01,0.9)) -sot=push(sot,taskFov) -sot=push(sot,taskBase) -sot=push(sot,taskRH) -sot=push(sot,taskLH) -sot.addContact(taskContact) -sot=push(sot,taskJL) +# 4.4 Base position +solver.push(taskJL) +solver.push(taskFov.task) +solver.push(taskBase.task) +solver.push(taskRH.task) +solver.push(taskWeight) + +#solver.push(taskChest.task) + +print ('Type go to run the solver loop') -print ('Type go to run the solver loop') \ No newline at end of file diff --git a/python/pr2_tests.py b/python/pr2_tests.py new file mode 100644 index 0000000..da87b00 --- /dev/null +++ b/python/pr2_tests.py @@ -0,0 +1,69 @@ +#This simple test allows to check the joints of the robot one by one. +# + +# 1. Init robot, ros binding, solver +from dynamic_graph.sot.pr2.pr2_tasks import * +from dynamic_graph.sot.pr2.robot import * +from dynamic_graph.sot.core.robot_simu import RobotSimu +from dynamic_graph.sot.core.meta_tasks_kine import gotoNd +from dynamic_graph import plug + +robot = Pr2('pr2', device=RobotSimu('pr2')) +plug(robot.device.state, robot.dynamic.position) + +from dynamic_graph.ros import * +ros = Ros(robot) + +from dynamic_graph.sot.dyninv import SolverKine +solver = initialize(robot, SolverKine) + + +# 2. Main loop +from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts +dt=3e-3 +@loopInThread +def inc(): + robot.device.increment(dt) + +runner=inc() +[go,stop,next,n]=loopShortcuts(runner) + + + +#### Build the stack of tasks +taskBase = Pr2BaseTask(robot) +gotoNd(taskBase, (0,0,0,0,0,0), '100011') +solver.push(taskBase.task) + + +# 3. Init Tasks +initPostureTask(robot) +solver.push(robot.tasks['robot_task_position']) + +# set Zero to all the joints. +def setZero(robot): + robot.features['featurePosition'].posture.value = (0,)* len(robot.halfSitting) + +# set Zero to all the joints. +def setInitial(robot): + robot.features['featurePosition'].posture.value =robot.halfSitting + +# allows to change only one joint and to test +def testJoint(robot, index, angle): + currentState = robot.dynamic.position.value + #state(index) = angle + posture = currentState[0:index] + (angle,) + currentState[index+1: len(robot.halfSitting)] + robot.features['featurePosition'].posture.value = posture + +# Set position +def setRootPosition(robot, root): + homo = root[0:2] + (0,0,0,) + (root[2],) + print homo + gotoNd(taskBase, homo, '100011') + +def help(): + print 'setZero(robot): set Zero to all the joints.' + print 'setInitial(robot): reset to the initial position' + print 'testJoint(robot, index, angle): set the joint(index) to the value angle' + print 'setRootPosition(robot, root): defines the root position (X,Y,theta)' + diff --git a/robots/pr2_light.srdf b/robots/pr2_light.srdf new file mode 100644 index 0000000..2bae16b --- /dev/null +++ b/robots/pr2_light.srdf @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robots/pr2_light.urdf.xacro b/robots/pr2_light.urdf.xacro new file mode 100644 index 0000000..8505a1f --- /dev/null +++ b/robots/pr2_light.urdf.xacro @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/robots/upload_pr2.launch b/robots/upload_pr2.launch new file mode 100644 index 0000000..fc7662b --- /dev/null +++ b/robots/upload_pr2.launch @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 3a30b17..607e914 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -7,18 +7,12 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES) SET_TARGET_PROPERTIES(${NAME} PROPERTIES PREFIX "") PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph) - PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mathtools) - PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mal) PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core) INSTALL(TARGETS ${NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin) # build python submodule STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${NAME}) - #ADD_DEPENDENCIES(${NAME} MKDIR_${PYTHON_LIBRARY_NAME}) - #ADD_CUSTOM_TARGET(MKDIR_${PYTHON_LIBRARY_NAME} - #mkdir -p dynamic_graph/sot/pr2/${PYTHON_LIBRARY_NAME} - #) SET(NEW_ENTITY_CLASS ${ENTITIES}) DYNAMIC_GRAPH_PYTHON_MODULE("sot/pr2/${PYTHON_LIBRARY_NAME}" ${NAME} diff --git a/src/dynamic_graph/sot/pr2/pr2_tasks.py b/src/dynamic_graph/sot/pr2/pr2_tasks.py index 16bafb3..b463f1a 100644 --- a/src/dynamic_graph/sot/pr2/pr2_tasks.py +++ b/src/dynamic_graph/sot/pr2/pr2_tasks.py @@ -1,81 +1,186 @@ -from numpy import eye, array +from numpy import eye, array, diag from dynamic_graph import plug from dynamic_graph.sot.core import * from dynamic_graph.sot.dynamics import * from dynamic_graph.sot.pr2.robot import * -from dynamic_graph.sot.dyninv import SolverKine from dynamic_graph.sot.core.meta_task_6d import toFlags from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d from dynamic_graph.sot.dyninv import TaskInequality, TaskJointLimits from dynamic_graph.sot.core.meta_task_visual_point import MetaTaskVisualPoint -from dynamic_graph.ros import * -def toList(sot): - return map(lambda x: x[1:-1],sot.dispStack().split('|')[1:]) - -def SotPr2(robot): - SolverKine.toList = toList - sot = SolverKine('sot') - sot.setSize(robot.dimension) - return sot - -def push(sot,task): - if isinstance(task,str): taskName=task - elif "task" in task.__dict__: taskName=task.task.name - else: taskName=task.name - if taskName not in sot.toList(): - sot.push(taskName) - if taskName!="taskposture" and "taskposture" in sot.toList(): - sot.down("taskposture") - return sot - -def pop(sot,task): - if isinstance(task,str): taskName=task - elif "task" in task.__dict__: taskName=task.task.name - else: taskName=task.name - if taskName in sot.toList(): sot.rm(taskName) - return sot - -def initializePr2(robot,sot): - robot.dynamic.velocity.value = robot.dimension*(0.,) - robot.dynamic.acceleration.value = robot.dimension*(0.,) - robot.dynamic.ffposition.unplug() - robot.dynamic.ffvelocity.unplug() - robot.dynamic.ffacceleration.unplug() - robot.dynamic.setProperty('ComputeBackwardDynamics','true') - robot.dynamic.setProperty('ComputeAccelerationCoM','true') - robot.device.control.unplug() - plug(sot.control,robot.device.control) - return [robot,sot] - -def initPr2RosSimuProblem(): - robot = Pr2('pr2', device=RobotSimu('pr2')) - plug(robot.device.state, robot.dynamic.position) - ros = Ros(robot) - sot = SotPr2(robot) - [robot,sot] = initializePr2(robot,sot) - return [robot,ros,sot] - +from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver, createCenterOfMassFeatureAndTask, createOperationalPointFeatureAndTask, initializeSignals + + +# +def initialize (robot, solverType=SOT): + """ + Initialize the solver, and define shortcuts for the operational points + """ + + # TODO: this should disappear in the end. + # --- center of mass ------------ + (robot.featureCom, robot.featureComDes, robot.comTask) = \ + createCenterOfMassFeatureAndTask(robot, + '{0}_feature_com'.format(robot.name), + '{0}_feature_ref_com'.format(robot.name), + '{0}_task_com'.format(robot.name)) + + # --- operational points tasks ----- + robot.features = dict() + robot.tasks = dict() + for op in robot.OperationalPoints: + (robot.features[op], robot.tasks[op]) = \ + createOperationalPointFeatureAndTask(robot, + op, '{0}_feature_{1}'.format(robot.name, op), + '{0}_task_{1}'.format(robot.name, op)) + # define a member for each operational point + w = op.split('-') + memberName = w[0] + for i in w[1:]: + memberName += i.capitalize() + setattr(robot, memberName, robot.features[op]) + + initializeSignals (robot) + + # --- create solver --- # + solver = Solver (robot, solverType) + + # --- push balance task --- # + metaContact = Pr2ContactTask(robot) + robot.tasks ['contact'] = metaContact.task + robot.features ['contact'] = metaContact.feature + metaContact.feature.selec.value = '011100' + metaContact.featureDes.position.value = \ + array([[1,0,0,0],[0,1,0,0],[0,0,1,0.051],[0,0,0,1]]) + solver.push(robot.tasks['contact']) +# solver.sot.addContact(robot.tasks['contact']) + + return solver -# -- HANDS ------------------------------------------------------------------ - -Pr2handMgrip = eye(4) -Pr2handMgrip[0:3,3] = (0.18,0,0) +# -- HANDS ------------------------------------------------------------------ +# TODO: directly use the feature. def Pr2RightHandTask(robot): - task=MetaTaskKine6d('rh',robot.dynamic,'rh','right-wrist') - task.opmodif = matrixToTuple(Pr2handMgrip) + task=MetaTaskKine6d('right-wrist',robot.dynamic,'right-wrist','right-wrist') task.feature.frame('desired') return task - + def Pr2LeftHandTask(robot): - task=MetaTaskKine6d('lh',robot.dynamic,'lh','left-wrist') - task.opmodif = matrixToTuple(Pr2handMgrip) + task=MetaTaskKine6d('left-wrist',robot.dynamic,'left-wrist','left-wrist') + task.feature.frame('desired') + return task + +# -- GRIPPERS -------------------------------------------------------------- +class Pr2Gripper: + feature = None + featureDes = None + task = None + + """ + name: + robot the robot + index: index of the first gripper value in the state vector + dim: dimension of the gripper + """ + def __init__(self, name, robot, index): + dim=1 + self.feature = FeatureGeneric('feature'+name) + self.featureDes = FeatureGeneric('featureDes'+name) + self.feature.setReference('featureDes'+name) + self.featureDes.errorIN.value = (0,) * dim; + + # create jacobian. + jacobianGripper = eye(dim,robot.dimension) * 0; + jacobianGripper[0][index] = 1; + self.feature.jacobianIN.value = jacobianGripper + + # only selec some dofs + selecRightGripper = Selec_of_vector('selec'+name) + selecRightGripper.selec(index, index+dim) + plug(robot.dynamic.position, selecRightGripper.sin) + plug(selecRightGripper.sout, self.feature.errorIN) + + # 2\ Define the task. Associate to the task the position feature. + self.task = Task('task'+name) + self.task.add('feature'+name) + self.task.controlGain.value = 1000 + + def open(self,gain=1000): + self.task.controlGain.value=gain + self.featureDes.errorIN.value=(0.08,)*1 + + def close(self,gain=1000): + self.task.controlGain.value=gain + self.featureDes.errorIN.value=(0,)*1 + + def set(self, position,gain=1000): + self.task.controlGain.value=gain + self.featureDes.errorIN.value = (position,)*1 + + +def Pr2RightGripper(robot): + gripper = Pr2Gripper('RightGripper', robot,49) + return gripper + +def Pr2LeftGripper(robot): + gripper = Pr2Gripper('LeftGripper', robot,34) + return gripper + +# -- CHEST ------------------------------------------------------------------ +def Pr2ChestTask(robot): + task=MetaTaskKine6d('chest',robot.dynamic,'chest','chest') task.feature.frame('desired') return task +""" Create a task that aims at reducing the contribution of the some of the joints.""" +def Pr2Weight(robot): + # --- TASK POSTURE -------------------------------------------------- + # set a default position for the joints. + feature = FeatureGeneric('feature_weight') + weight = 18 * (0,) + (500,)* 1 + (51-19) * (1,) + feature.jacobianIN.value = diag(weight) + feature.errorIN.value = 51 * (0,) + + task=Task('weight') + task.add('feature_weight') + + gainWeight = GainAdaptive('gain_weight') + gainWeight.set(0.1,0.1,125e3) + gainWeight.gain.value = 5 + plug(task.error, gainWeight.error) + plug(gainWeight.gain, task.controlGain) + + return (task, feature) + +def initPostureTask(robot): + # --- TASK POSTURE -------------------------------------------------- + # set a default position for the joints. + robot.features['featurePosition'] = FeaturePosture('featurePosition') + plug(robot.device.state,robot.features['featurePosition'].state) + robotDim = len(robot.dynamic.velocity.value) + robot.features['featurePosition'].posture.value = robot.halfSitting + + postureTaskDofs = [True]*6 + [False]*(51-6) + postureTaskDofs = [True]*(51) + + for dof,isEnabled in enumerate(postureTaskDofs): + if dof > 6: + robot.features['featurePosition'].selectDof(dof,isEnabled) + + robot.tasks['robot_task_position']=Task('robot_task_position') + robot.tasks['robot_task_position'].add('featurePosition') + # featurePosition.selec.value = toFlags((6,24)) + + gainPosition = GainAdaptive('gainPosition') + gainPosition.set(0.1,0.1,125e3) + gainPosition.gain.value = 5 + plug(robot.tasks['robot_task_position'].error,gainPosition.error) + plug(gainPosition.gain,robot.tasks['robot_task_position'].controlGain) + + + # -- CAMS ------------------------------------------------------------------ Pr2headMcam = array([[0.0,0.0,1.0,0.0],[1.,0.0,0.0,0.0],[0.0,1.,0.0,0.0],[0.0,0.0,0.0,1.0]]) @@ -120,35 +225,27 @@ def Pr2ContactTask(robot): task = MetaTaskKine6d('contact',robot.dynamic,'contact','left-ankle') task.feature.frame('desired') task.feature.selec.value = '011100' + task.featureDes.position.value = \ + array([[1,0,0,0],[0,1,0,0],[0,0,1,0.051],[0,0,0,1]]) task.gain.setConstant(10) #locals()['contact'] = task return task - - -def Pr2FixedContactTask(robot): - task = MetaTaskKine6d('contactFixed',robot.dynamic,'contact','left-ankle') - task.feature.frame('desired') - task.gain.setConstant(10) - #task.feature.selec.value = '111111' - #locals()['contactFixed'] = task - return task - # -- WAIST ------------------------------------------------------------------ -baseMground=eye(4); -baseMground[0:3,3] = (0,0,-0.051) - def Pr2BaseTask(robot): task = MetaTaskKine6d('base',robot.dynamic,'waist','waist') + baseMground=eye(4); + baseMground[0:3,3] = (0,0,0) task.opmodif = matrixToTuple(baseMground) task.feature.frame('desired') - task.feature.selec.value = '011100' + task.feature.selec.value = '100011' task.gain.setConstant(10) return task -__all__ = ["SotPr2", "Pr2RightHandTask", "Pr2LeftHandTask", "Pr2GazeTask", - "Pr2FoVTask", "Pr2JointLimitsTask", "Pr2ContactTask", "toList", - "Pr2FixedContactTask", "initPr2RosSimuProblem", "push", "pop", - "Pr2BaseTask"] +__all__ = ["Pr2RightHandTask", "Pr2LeftHandTask", "Pr2GazeTask", + "Pr2FoVTask", "Pr2JointLimitsTask", "Pr2ContactTask", + "Pr2RightGripper", "Pr2LeftGripper", + "initialize", "Pr2ChestTask", "Pr2Weight", + "Pr2BaseTask", "initPostureTask"] diff --git a/src/dynamic_graph/sot/pr2/prologue.py b/src/dynamic_graph/sot/pr2/prologue.py index abc61e1..0620fd6 100644 --- a/src/dynamic_graph/sot/pr2/prologue.py +++ b/src/dynamic_graph/sot/pr2/prologue.py @@ -15,33 +15,18 @@ # received a copy of the GNU Lesser General Public License along with # dynamic-graph. If not, see . -print("sot_pr2") -print("Compiled for robot PR-2.") +print("Prologue PR2") -from dynamic_graph import plug -from robot import Pr2 from dynamic_graph.entity import PyEntityFactoryClass +from dynamic_graph.sot.pr2.robot import Pr2 Device = PyEntityFactoryClass('Pr2Device') -robot = Pr2(name = 'robot', device = Device('robot_device')) +robot = Pr2(name = 'robot', device = Device('PR2')) -# FIXME: this must be set so that the graph can be evaluated. -#robot.device.zmp.value = (0., 0., 0.) +#todo: necessary? +#plug(robot.device.state, robot.dynamic.position) -# Create a solver. -from dynamic_graph.sot.dyninv import SolverKine -def toList(solver): - return map(lambda x: x[1:-1],solver.dispStack().split('|')[1:]) -SolverKine.toList = toList -solver = SolverKine('sot') -solver.setSize(robot.dimension) -robot.device.control.unplug() -plug(solver.control,robot.device.control) -plug(robot.device.state,robot.dynamic.position) - -print("Prologue ran successfully.") - -# Make sure only robot and solver are visible from the outside. -__all__ = ["robot", "solver"] +# Make sure only robot is visible from the outside. +__all__ = ["robot"] diff --git a/src/dynamic_graph/sot/pr2/robot.py b/src/dynamic_graph/sot/pr2/robot.py index 45845ef..f83f003 100644 --- a/src/dynamic_graph/sot/pr2/robot.py +++ b/src/dynamic_graph/sot/pr2/robot.py @@ -25,9 +25,16 @@ class Pr2(AbstractHumanoidRobot): """ OperationalPoints = ['right-wrist','left-wrist','waist','gaze','chest','left-ankle'] - - SpecialLinks = ['BODY', 'l_wrist', 'r_wrist', 'l_gripper', 'r_gripper', 'gaze','torso','l_ankle'] - SpecialNames = ['base_link', 'l_wrist_roll_link', 'r_wrist_roll_link', 'l_gripper_palm_link', 'r_gripper_palm_link', 'double_stereo_link','torso_lift_link','base_link'] + + jointMap={} + jointMap['BODY'] = 'base_link' + jointMap['l_wrist'] = 'l_gripper_palm_link' + jointMap['r_wrist'] = 'r_gripper_palm_link' + jointMap['l_gripper'] = 'l_gripper_tool_frame' + jointMap['r_gripper'] = 'r_gripper_tool_frame' + jointMap['gaze'] = 'double_stereo_link' + jointMap['torso'] = 'torso_lift_link' + jointMap['l_ankle'] = 'base_link' # TODO? tracedSignals = { 'dynamic': ["com", "position", "velocity", "acceleration"], @@ -35,20 +42,34 @@ class Pr2(AbstractHumanoidRobot): } def specifySpecialLinks(self): - if len(self.SpecialLinks) == len(self.SpecialNames): - for i in range(0,len(self.SpecialLinks)): - self.dynamic.addJointMapping(self.SpecialLinks[i], self.SpecialNames[i]) - else: - print 'No Special joints added : SpecialLinks.size != SpecialJoints.size' + for i in self.jointMap: + self.dynamic.addJointMapping(i, self.jointMap[i]) def __init__(self, name, device = None, tracer = None): AbstractHumanoidRobot.__init__ (self, name, tracer) self.device = device self.dynamic = RosRobotModel("{0}_dynamic".format(name)) self.specifySpecialLinks() + #Note: the param 'robot_description' should be defined before the next instruction self.dynamic.loadFromParameterServer() self.dimension = self.dynamic.getDimension() self.halfSitting = (0.,) * self.dimension + lst = list(self.halfSitting) + lst[24] = -0.33 + lst[26] = -0.47 + lst[39] = -0.33 + lst[41] = -0.47 + self.halfSitting = tuple(lst) + + # correct the initialization of the dynamic. + self.dynamic.velocity.value = self.dimension*(0.,) + self.dynamic.acceleration.value = self.dimension*(0.,) + self.dynamic.ffposition.unplug() + self.dynamic.ffvelocity.unplug() + self.dynamic.ffacceleration.unplug() + self.dynamic.setProperty('ComputeBackwardDynamics','true') + self.dynamic.setProperty('ComputeAccelerationCoM','true') + self.initializeRobot() __all__ = ["Pr2"] diff --git a/src/fake_controller.cpp b/src/fake_controller.cpp new file mode 100644 index 0000000..b21fc26 --- /dev/null +++ b/src/fake_controller.cpp @@ -0,0 +1,39 @@ +#include "sot_pr2/pr2_controller_plugin.h" +#include +#include + +boost::condition cond2; +boost::mutex mut; + +struct timeval t0, t1, t2, t3; + +void +controller(sot_pr2::Pr2ControllerPlugin *pr2) { + // Setup + gettimeofday(&t0,0); + pr2->starting(); + + while(true) { + gettimeofday(&t1, 0); + long elapsed = (t1.tv_sec-t0.tv_sec)*1000000 + t1.tv_usec-t0.tv_usec; + if (elapsed >= 1000) { + t0 = t1; + gettimeofday(&t2,0); + pr2->update(); + gettimeofday(&t3,0); + long elapsed2 = (t3.tv_sec-t2.tv_sec)*1000000 + t3.tv_usec-t2.tv_usec; + std::cout << "[FakeController] : " << elapsed2 << " µs" << std::endl; + } + //boost::this_thread::sleep(boost::posix_time::microseconds(1)); + } + + pr2->stopping(); +} + +int main(void) { + sot_pr2::Pr2ControllerPlugin pr2; + ros::NodeHandle n("sot_pr2_fake_controller"); + pr2.init(0,n); + boost::thread control_loop(controller,&pr2); + control_loop.join(); +} diff --git a/src/pr2_controller_plugin.cpp b/src/pr2_controller_plugin.cpp new file mode 100644 index 0000000..352faa4 --- /dev/null +++ b/src/pr2_controller_plugin.cpp @@ -0,0 +1,277 @@ +#include "sot_pr2/pr2_controller_plugin.h" +#include +#include + + +namespace sot_pr2 { + +std::ofstream logout; + +Pr2ControllerPlugin::Pr2ControllerPlugin() + : pr2_controller_interface::Controller(), + sot_controller_(), + loop_count_(0), + robot_(NULL) { + logout.open("/tmp/out.log", std::ios::out); + + // Grippers init + r_gripper_client_ = new actionlib::SimpleActionClient("r_gripper_controller/gripper_action", true); + l_gripper_client_ = new actionlib::SimpleActionClient("l_gripper_controller/gripper_action", true); + r_gripper_position = 0; + l_gripper_position = 0; +} + +Pr2ControllerPlugin::~Pr2ControllerPlugin() { + delete r_gripper_client_; + delete l_gripper_client_; +} + +bool +Pr2ControllerPlugin::init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n) { + sot_controller_.node_ = n; + cmd_vel_pub_ = sot_controller_.node_.advertise("/base_controller/command", 1); + + // Check initialization + if (!robot) { + ROS_ERROR_STREAM("NULL robot pointer"); + return false; + } + if (!robot->model_) { + ROS_ERROR_STREAM("NULL model pointer"); + return false; + } + robot_ = robot; + + // Get the joints + XmlRpc::XmlRpcValue joint_names; + if (!sot_controller_.node_.getParam("joints", joint_names)) { + ROS_ERROR("No joints given. (namespace: %s)", sot_controller_.node_.getNamespace().c_str()); + return false; + } + if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray) { + ROS_ERROR("Malformed joint specification. (namespace: %s)", sot_controller_.node_.getNamespace().c_str()); + return false; + } + for (int i=0; igetJointState((std::string)name_value)); + if (!j) { + ROS_ERROR("Joint not found: %s. (namespace: %s)", ((std::string)name_value).c_str(), sot_controller_.node_.getNamespace().c_str()); + return false; + } + joints_.push_back(j); + } + + // Ensures joints are calibrated + for (size_t i=0; icalibrated_) { + ROS_ERROR("Joint %s was not calibrated (namespace: %s)", joints_[i]->joint_->name.c_str(), sot_controller_.node_.getNamespace().c_str()); + return false; + } + } + + // Setup PID controllers + std::string gains_ns; + if (!sot_controller_.node_.getParam("gains", gains_ns)) + gains_ns = sot_controller_.node_.getNamespace() + "/gains"; + pids_.resize(joints_.size()); + for (size_t i=0; ijoint_->name))) { + if (!pids_[i].init(ros::NodeHandle(sot_controller_.node_,"pid_parameters"))) { + ROS_ERROR("Failed to build PID controller"); + return false; + } + } + } + + // TF Listener + listener_.waitForTransform("odom_combined", "base_footprint", ros::Time(0), ros::Duration(1.0)); + + // Allocate space + const unsigned int jsz = joints_.size(); + joint_encoder_.resize(jsz); + joint_velocity_.resize(jsz); + joint_control_.resize(jsz); + error_raw.resize(jsz); + error.resize(jsz); + + controller_state_publisher_.reset( + new realtime_tools::RealtimePublisher + (sot_controller_.node_, "state", 1)); + controller_state_publisher_->lock(); + for (size_t j=0; jmsg_.joint_names.push_back(joints_[j]->joint_->name); + controller_state_publisher_->msg_.desired.positions.resize(joints_.size()); + controller_state_publisher_->msg_.desired.velocities.resize(joints_.size()); + controller_state_publisher_->msg_.desired.accelerations.resize(joints_.size()); + controller_state_publisher_->msg_.actual.positions.resize(joints_.size()); + controller_state_publisher_->msg_.actual.velocities.resize(joints_.size()); + controller_state_publisher_->msg_.error.positions.resize(joints_.size()); + controller_state_publisher_->msg_.error.velocities.resize(joints_.size()); + controller_state_publisher_->unlock(); + + timeFromStart_ = 0.0; + + // Start Pr2 Sot Controller + sot_controller_.init(); + + return true; +} + +void +Pr2ControllerPlugin::fillSensors() { + // Joint values + sensorsIn_["joints"].setName("position"); + for (unsigned int i=0; iposition_; + sensorsIn_["joints"].setValues(joint_encoder_); + + // Joint velocities + sensorsIn_["velocities"].setName("velocity"); + for (unsigned int i=0; ivelocity_; + sensorsIn_["velocities"].setValues(joint_velocity_); + + // Get Odometry + tf::StampedTransform current_transform; + listener_.lookupTransform("odom_combined", "base_footprint", ros::Time(0), current_transform); + std::vector odom(6); + tf::Vector3 xyz = current_transform.getOrigin(); + tf::Quaternion q = current_transform.getRotation(); + odom[0] = xyz[0]; + odom[1] = xyz[1]; + odom[2] = 0.0; + odom[3] = 0.0; + odom[4] = 0.0; + odom[5] = std::atan2(2*(q.w()*q.z() + q.x()*q.y()), 1 - 2*(q.y()*q.y() + q.z()*q.z())); + sensorsIn_["odometry"].setValues(odom); +} + +void +Pr2ControllerPlugin::readControl() { + ros::Time time = robot_->getTime(); + ros::Duration dt = time - last_time_; + last_time_ = time; + + // Update command + joint_control_ = controlValues_["joints"].getValues(); + joint_velocity_ = controlValues_["velocities"].getValues(); + // 0-11 are casters and are controled by base controller + for (unsigned int i=12; iposition_ - joint_control_[i]; + double errord = joints_[i]->velocity_ - joint_velocity_[i]; + joints_[i]->commanded_effort_ += pids_[i].updatePid(error[i], errord, dt); + } + + // Base controller + geometry_msgs::Twist base_cmd; + std::vector vel = controlValues_["ffvelocity"].getValues(); + std::vector ff = controlValues_["baseff"].getValues(); + if (ff.size() == 12) { + base_cmd.linear.x = base_cmd.linear.y = base_cmd.linear.z = 0; + base_cmd.angular.x = base_cmd.angular.y = base_cmd.angular.z = 0; + base_cmd.linear.x = ff[0]*vel[0] + ff[1]*vel[1]; + base_cmd.linear.y = ff[4]*vel[0] + ff[5]*vel[1]; + base_cmd.angular.z = ff[10]*vel[5]; + } + cmd_vel_pub_.publish(base_cmd); + + // Grippers controller + const int r_gripper = 43; + const int l_gripper = 28; + pr2_controllers_msgs::Pr2GripperCommandGoal rgoal, lgoal; + rgoal.command.position = joint_control_[r_gripper]; + rgoal.command.max_effort = -1.0; + lgoal.command.position = joint_control_[l_gripper]; + lgoal.command.max_effort = -1.0; + + r_gripper_client_->sendGoal(rgoal); + l_gripper_client_->sendGoal(lgoal); + + // State publishing + if (loop_count_ % 10 == 0) { + if (controller_state_publisher_ && controller_state_publisher_->trylock()) { + controller_state_publisher_->msg_.header.stamp = time; + for (size_t j=0; jmsg_.desired.positions[j] = joint_control_[j]; + controller_state_publisher_->msg_.actual.positions[j] = joints_[j]->position_; + controller_state_publisher_->msg_.actual.velocities[j] = joints_[j]->velocity_; + controller_state_publisher_->msg_.actual.time_from_start= ros::Duration(timeFromStart_); + controller_state_publisher_->msg_.error.positions[j] = error[j]; + } + controller_state_publisher_->unlockAndPublish(); + } + } + ++loop_count_; +} + + +void +Pr2ControllerPlugin::starting() { + std::cout << "STARTING" << std::endl; + last_time_ = robot_->getTime(); + + for (size_t i=0; i #include +#include namespace sot_pr2 { @@ -9,59 +10,168 @@ const double Pr2Device::TIMESTEP_DEFAULT = 0.001; Pr2Device::Pr2Device(const std::string &name) : dynamicgraph::sot::Device(name), - timestep_(TIMESTEP_DEFAULT) { + timestep_(TIMESTEP_DEFAULT), + previous_state_(), + robotState_ ("StackOfTasks(" + name + ")::output(vector)::robotState"), + pose(), + baseff_(), + loop_count_(0) +{ + sotDEBUGIN(25); + signalRegistration(robotState_); + baseff_.resize(12); + + std::string docstring; + docstring = + "\n" + " Integrate dynamics for time step provided as input\n" + "\n" + " take one floating point number as input\n" + "\n"; + addCommand("increment", + dynamicgraph::command::makeCommandVoid1((Device&)*this, + &Device::increment, docstring)); + + sotDEBUGOUT(25); } Pr2Device::~Pr2Device() { } +void +Pr2Device::setSensors(SensorMap &sensorsIn) { + sotDEBUGIN(25); + SensorMap::iterator it; + + // Joints positions + it = sensorsIn.find("joints"); + if (it != sensorsIn.end()) { + const std::vector &anglesIn = it->second.getValues(); + state_.resize(anglesIn.size() + 6); + for (unsigned i=0;i<6; ++i) + state_(i) = 0.; + for (unsigned i=0; i &velIn = it->second.getValues(); + velocity_.resize(velIn.size() + 6); + for (unsigned i=0;i<6; ++i) + velocity_(i) = 0.; + for (unsigned i=0; i odomIn = it->second.getValues(); + try { + for (unsigned i=0; i<6; ++i) + state_(i) = odomIn[i]; + } + catch (...) {} + } -bool -Pr2Device::init() { - return true; + sotDEBUGOUT(25); } void -Pr2Device::setup(jointMap_t &jm) { - int t = stateSOUT.getTime() + 1; - maal::boost::Vector state = stateSOUT.access(t); - - sotDEBUG (25) << "stateSOUT.access (" << t << ") = " << state << std::endl; - sotDEBUG (25) << "state.size () = " << state.size () << std::endl; - sotDEBUG (25) << "joints_.size () = " << jm.size () << std::endl; - - unsigned jointId = 0; - jointMap_t::const_iterator it; - for (it=jm.begin(); it!=jm.end(); ++it, ++jointId) { - if (jointId+6 >= state.size() || !it->second.second) - continue; - state(jointId+6) = it->second.second->position_; - } +Pr2Device::setupSetSensors(SensorMap &sensorsIn) { + setSensors(sensorsIn); +} - ROS_INFO_STREAM("state : " << state); +void +Pr2Device::nominalSetSensors(SensorMap &sensorsIn) { + setSensors(sensorsIn); +} - stateSOUT.setConstant(state); - state_ = state; +void +Pr2Device::cleanupSetSensors(SensorMap &sensorsIn) { + setSensors(sensorsIn); } void -Pr2Device::control(jointMap_t &jm) { - try { - increment(timestep_); +Pr2Device::getControl(ControlMap &controlOut) { + sotDEBUGIN(25); + std::vector anglesOut; + anglesOut.resize(state_.size()); + std::vector velocitiesOut; + velocitiesOut.resize(state_.size()); + + try { increment(timestep_); } + catch (std::exception & e) { + std::cerr <<" Pr2Device::getControl " << e.what() << std::endl; } - catch (...) {} + //++loop_count_; + + sotDEBUG(25) << "state = " << state_ << std::endl; + sotDEBUG(25) << "diff = " << ((previous_state_.size() == state_.size()) ? + (state_ - previous_state_) : state_ ) << std::endl; + previous_state_ = state_; - sotDEBUG (25) << "state = " << state_ << std::endl; + // Get control - unsigned jointId = 0; - jointMap_t::const_iterator it; - for (it=jm.begin(); it!=jm.end(); ++it, ++jointId) { - if (jointId+6 >= state_.size() || !it->second.second) - continue; - it->second.second->commanded_effort_ = state_(jointId+6); + ml::Vector control; + try { + control = controlSIN.accessCopy(); } + catch (...) { + control.resize(state_.size()); + for (unsigned i=0; i ffvelocity(6,0.); + for (int i=0; i<6; ++i) + ffvelocity[i] = control(i); + controlOut["ffvelocity"].setValues(ffvelocity); + + sotDEBUGOUT(25); } +void +Pr2Device::updateRobotState(const std::vector &anglesIn) +{ + sotDEBUGIN(25); + for (unsigned i=0; i #include +#include +#include + +#include +#include + +#define ROBOTNAME std::string("PR2") + namespace sot_pr2 { const std::string Pr2SotController::LOG_PYTHON="/tmp/pr2_sot_controller.out"; +#define LOG_TRACE(x) sotDEBUG(25) << __FILE__ << ":" << __FUNCTION__ <<"(#" << __LINE__ << " ) " << x << std::endl -Pr2SotController::Pr2SotController() -: pr2_controller_interface::Controller(), - interpreter_(dynamicgraph::rosInit(false)), - joint_map_(), - device_(new Pr2Device("robot_device")) { +boost::condition_variable cond; +boost::mutex mut; +bool data_ready; + +void +workThread(Pr2SotController *actl) { + dynamicgraph::Interpreter aLocalInterpreter(actl->node_); + actl->interpreter_ = boost::make_shared(aLocalInterpreter); + + std::cout << "Going through the thread." << std::endl; + { + boost::lock_guard lock(mut); + data_ready=true; + } + cond.notify_all(); + ros::waitForShutdown(); } -void Pr2SotController::runPython(std::ostream& file, const std::string& command) +Pr2SotController::Pr2SotController() +: node_(dynamicgraph::rosInit(false,true)) +, device_(ROBOTNAME) { + std::cout << "Going through Pr2SotController." << std::endl; + boost::thread thr(workThread,this); + LOG_TRACE(""); + boost::unique_lock lock(mut); + cond.wait(lock); + startupPython(); + interpreter_->startRosService (); +} + +Pr2SotController::~Pr2SotController() { +} + +void +Pr2SotController::setupSetSensors(SensorMap &sensorsIn) { + device_.setupSetSensors(sensorsIn); +} + +void +Pr2SotController::nominalSetSensors(SensorMap &sensorsIn) { + device_.nominalSetSensors(sensorsIn); +} + +void +Pr2SotController::cleanupSetSensors(SensorMap &sensorsIn) { + device_.cleanupSetSensors(sensorsIn); +} + +void +Pr2SotController::getControl(ControlMap &controlOut) { + try { + LOG_TRACE(""); + device_.getControl(controlOut); + LOG_TRACE(""); + } + catch (dynamicgraph::sot::ExceptionAbstract &err) { + LOG_TRACE(err.getStringMessage()); + throw err; + } +} + + +void +Pr2SotController::runPython(std::ostream &file, + const std::string &command, + dynamicgraph::Interpreter &interpreter) { file << ">>> " << command << std::endl; std::string lerr(""),lout(""),lres(""); - interpreter_.runCommand(command,lres,lout,lerr); + interpreter.runCommand(command,lres,lout,lerr); if (lres != "None") { if (lres=="") { file << lout << std::endl; @@ -32,83 +99,40 @@ void Pr2SotController::runPython(std::ostream& file, const std::string& command) } } -/// Controller initialization in non-realtime -bool Pr2SotController::init(pr2_mechanism_model::RobotState *robot, - ros::NodeHandle &nh) -{ - // Check initialization - if (!robot) { - ROS_ERROR_STREAM("NULL robot pointer"); - return false; - } - - if (!robot->model_) { - ROS_ERROR_STREAM("NULL model pointer"); - return false; - } - - // Fill joint map - std::map::const_iterator it; - for (it=robot->model_->robot_model_.joints_.begin(); it!=robot->model_->robot_model_.joints_.end();++it) { - Pr2JointPtr state(robot->getJointState(it->first)); - joint_map_[it->first] = std::make_pair(it->second, state); - } - - // Init Device - if (!device_->init()) { - ROS_ERROR_STREAM("Device failed to initialize"); - return false; - } - - // Bind with SoT - try { - std::ofstream aof(LOG_PYTHON.c_str()); - runPython (aof, "import sys, os"); - runPython (aof, "pythonpath = os.environ['PYTHONPATH']"); - runPython (aof, "path = []"); - runPython (aof, "for p in pythonpath.split(':'):\n" - " if p not in sys.path:\n" - " path.append(p)"); - runPython (aof, "path.extend(sys.path)"); - runPython (aof, "sys.path = path"); - runPython (aof, "from dynamic_graph.sot.pr2.prologue import robot, solver"); - - interpreter_.startRosService (); - } - catch (const std::exception &e) { - ROS_ERROR_STREAM("Failed to initialize controller: " << e.what()); - return false; - } - catch (...) { - ROS_ERROR_STREAM("Failed to initialize controller: Unknown exception."); - return false; - } - - return true; +void +Pr2SotController::startupPython() { + std::ofstream aof(LOG_PYTHON.c_str()); + runPython (aof, "import sys, os", *interpreter_); + runPython (aof, "pythonpath = os.environ['PYTHONPATH']", *interpreter_); + runPython (aof, "path = []", *interpreter_); + runPython (aof, "for p in pythonpath.split(':'):\n" + " if p not in sys.path:\n" + " path.append(p)", *interpreter_); + runPython (aof, "path.extend(sys.path)", *interpreter_); + runPython (aof, "sys.path = path", *interpreter_); + runPython (aof, "from dynamic_graph.sot.pr2.prologue import robot", *interpreter_); + + dynamicgraph::rosInit(true); + + aof.close(); } - -/// Controller startup in realtime -void Pr2SotController::starting() -{ - device_->setup(joint_map_); } - -/// Controller update loop in realtime -void Pr2SotController::update() +extern "C" { - device_->control(joint_map_); + dynamicgraph::sot::AbstractSotExternalInterface * createSotExternalInterface() + { + return new sot_pr2::Pr2SotController; + } } - -/// Controller stopping in realtime -void Pr2SotController::stopping() +extern "C" { + void destroySotExternalInterface(dynamicgraph::sot::AbstractSotExternalInterface *p) + { + delete p; + } } -} // namespace -/// Register controller to pluginlib -PLUGINLIB_EXPORT_CLASS(sot_pr2::Pr2SotController, - pr2_controller_interface::Controller) diff --git a/src/pr2_threaded_sot_controller.cpp b/src/pr2_threaded_sot_controller.cpp new file mode 100644 index 0000000..9e8095c --- /dev/null +++ b/src/pr2_threaded_sot_controller.cpp @@ -0,0 +1,216 @@ +#include "sot_pr2/pr2_threaded_sot_controller.h" +#include +#include + +#include +#include + +#include +#include + +#include + +#define ROBOTNAME std::string("PR2") + +namespace sot_pr2 { + +const std::string Pr2ThreadedSotController::LOG_PYTHON="/tmp/pr2_sot_controller.out"; +#define LOG_TRACE(x) sotDEBUG(25) << __FILE__ << ":" << __FUNCTION__ <<"(#" << __LINE__ << " ) " << x << std::endl + +boost::condition_variable pr2_cond; +boost::condition cond2; +boost::mutex pr2_mut; +boost::mutex wait_start; +boost::mutex rmut, wmut; +bool pr2_data_ready; + +/*struct timeval t0, t1; +int iter;*/ + +void +workThread(Pr2ThreadedSotController *actl) { + dynamicgraph::Interpreter aLocalInterpreter(actl->node_); + actl->interpreter_ = boost::make_shared(aLocalInterpreter); + + std::cout << "Going through the thread." << std::endl; + { + boost::lock_guard lock(pr2_mut); + pr2_data_ready=true; + } + pr2_cond.notify_all(); + ros::waitForShutdown(); +} + +void +sampleAndHold(Pr2ThreadedSotController *actl) { + SensorMap deviceIn; + ControlMap deviceOut; + + // Wait the start flag + boost::unique_lock lock(wait_start); + cond2.wait(lock); + + // Go go go !!! + while (true) { + { + boost::mutex::scoped_lock lock(rmut); + deviceIn = actl->holdIn(); + } + actl->device()->nominalSetSensors(deviceIn); + try { + LOG_TRACE(""); + actl->device()->getControl(deviceOut); + LOG_TRACE(""); + } + catch (dynamicgraph::sot::ExceptionAbstract &err) { + LOG_TRACE(err.getStringMessage()); + throw err; + } + actl->device()->getControl(deviceOut); + { + boost::mutex::scoped_lock lock(wmut); + actl->holdOut() = deviceOut; + } + usleep(1); + //boost::this_thread::sleep(boost::posix_time::nanoseconds(1000)); + } +} + +Pr2ThreadedSotController::Pr2ThreadedSotController() +: node_(dynamicgraph::rosInit(false,true)) +, device_(ROBOTNAME) +{ + std::cout << "Going through Pr2ThreadedSotController." << std::endl; + boost::thread thr(workThread,this); + LOG_TRACE(""); + boost::unique_lock lock(pr2_mut); + pr2_cond.wait(lock); + startupPython(); + interpreter_->startRosService (); +} + +Pr2ThreadedSotController::~Pr2ThreadedSotController() { +} + +void +Pr2ThreadedSotController::init() { + boost::thread sampleAndHoldProcess(sampleAndHold,this); + //sampleAndHoldProcess.join(); +} + +void +Pr2ThreadedSotController::setupSetSensors(SensorMap &sensorsIn) { + /*gettimeofday(&t0, 0); + iter = 0;*/ + //device_.setupSetSensors(sensorsIn); + { + boost::mutex::scoped_lock lock(rmut); + _holdIn = sensorsIn; + } + cond2.notify_all(); +} + +void +Pr2ThreadedSotController::nominalSetSensors(SensorMap &sensorsIn) { + /*++iter; + gettimeofday(&t1, 0); + long elapsed = (t1.tv_sec-t0.tv_sec)*1000000 + t1.tv_usec-t0.tv_usec; + if (elapsed >= 1000000) { + double elapsed_s = elapsed / 1000000; + //std::cout << "[Pr2ThreadedSotController] " << iter / elapsed_s << " Hz" << std::endl; + t0 = t1; + iter = 0; + }*/ + //device_.nominalSetSensors(sensorsIn); + { + boost::mutex::scoped_lock lock(rmut); + _holdIn = sensorsIn; + } +} + +void +Pr2ThreadedSotController::cleanupSetSensors(SensorMap &sensorsIn) { + //device_.cleanupSetSensors(sensorsIn); + { + boost::mutex::scoped_lock lock(rmut); + _holdIn = sensorsIn; + } +} + +void +Pr2ThreadedSotController::getControl(ControlMap &controlOut) { + /*try { + LOG_TRACE(""); + device_.getControl(controlOut); + LOG_TRACE(""); + } + catch (dynamicgraph::sot::ExceptionAbstract &err) { + LOG_TRACE(err.getStringMessage()); + throw err; + }*/ + { + boost::mutex::scoped_lock lock(wmut); + controlOut = _holdOut; + } +} + + +void +Pr2ThreadedSotController::runPython(std::ostream &file, + const std::string &command, + dynamicgraph::Interpreter &interpreter) { + file << ">>> " << command << std::endl; + std::string lerr(""),lout(""),lres(""); + interpreter.runCommand(command,lres,lout,lerr); + if (lres != "None") { + if (lres=="") { + file << lout << std::endl; + file << "------" << std::endl; + file << lerr << std::endl; + + std::string err("Exception catched during sot controller initialization, please check the log file: " + LOG_PYTHON); + throw std::runtime_error(err); + } + else + file << lres << std::endl; + } +} + +void +Pr2ThreadedSotController::startupPython() { + std::ofstream aof(LOG_PYTHON.c_str()); + runPython (aof, "import sys, os", *interpreter_); + runPython (aof, "pythonpath = os.environ['PYTHONPATH']", *interpreter_); + runPython (aof, "path = []", *interpreter_); + runPython (aof, "for p in pythonpath.split(':'):\n" + " if p not in sys.path:\n" + " path.append(p)", *interpreter_); + runPython (aof, "path.extend(sys.path)", *interpreter_); + runPython (aof, "sys.path = path", *interpreter_); + runPython (aof, "from dynamic_graph.sot.pr2.prologue import robot", *interpreter_); + + dynamicgraph::rosInit(true); + + aof.close(); +} + +} + +/*extern "C" +{ + dynamicgraph::sot::AbstractSotExternalInterface * createSotExternalInterface() + { + return new sot_pr2::Pr2ThreadedSotController; + } +} + +extern "C" +{ + void destroySotExternalInterface(dynamicgraph::sot::AbstractSotExternalInterface *p) + { + delete p; + } +} + + +*/ diff --git a/urdf/base_v0/base.gazebo.xacro b/urdf/base_v0/base.gazebo.xacro new file mode 100644 index 0000000..c02c9db --- /dev/null +++ b/urdf/base_v0/base.gazebo.xacro @@ -0,0 +1,59 @@ + + + + + + + + + + + + + + + + + + + + + + + false + + true + 100.0 + + ${name}_link_geom + + + true + 100.0 + ${name}_bumper + + + + + + + + + true + 100.0 + ${name}_link + ${name}_pose_ground_truth + 0.01 + map + 0 0 0 + 0 0 0 + + ${name}_footprint + + + + + + + + diff --git a/urdf/base_v0/base.transmission.xacro b/urdf/base_v0/base.transmission.xacro new file mode 100644 index 0000000..9853f5c --- /dev/null +++ b/urdf/base_v0/base.transmission.xacro @@ -0,0 +1,24 @@ + + + + + + + + + ${reflect * 624/35 * 80/18} + + + + + + + + + ${-1 * 624/35 * 80/18} + + + diff --git a/urdf/base_v0/base.urdf.xacro b/urdf/base_v0/base.urdf.xacro new file mode 100644 index 0000000..3b1a12f --- /dev/null +++ b/urdf/base_v0/base.urdf.xacro @@ -0,0 +1,264 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/common.xacro b/urdf/common.xacro new file mode 100644 index 0000000..e86fea7 --- /dev/null +++ b/urdf/common.xacro @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/forearm_v0/forearm.gazebo.xacro b/urdf/forearm_v0/forearm.gazebo.xacro new file mode 100644 index 0000000..ce4e151 --- /dev/null +++ b/urdf/forearm_v0/forearm.gazebo.xacro @@ -0,0 +1,36 @@ + + + + + + + true + + + + + + true + + + + + + + + + + true + + + + + + + + + + diff --git a/urdf/forearm_v0/forearm.transmission.xacro b/urdf/forearm_v0/forearm.transmission.xacro new file mode 100644 index 0000000..f133fe2 --- /dev/null +++ b/urdf/forearm_v0/forearm.transmission.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + diff --git a/urdf/forearm_v0/forearm.urdf.xacro b/urdf/forearm_v0/forearm.urdf.xacro new file mode 100644 index 0000000..114a84c --- /dev/null +++ b/urdf/forearm_v0/forearm.urdf.xacro @@ -0,0 +1,130 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/gripper_v0/gripper.gazebo.xacro b/urdf/gripper_v0/gripper.gazebo.xacro new file mode 100644 index 0000000..c5dc499 --- /dev/null +++ b/urdf/gripper_v0/gripper.gazebo.xacro @@ -0,0 +1,285 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + + + + + + + + + + + + + + + + true + + + + + + + + + + + + + + + + true + false + + 100.0 + + ${prefix}_l_finger_tip_link_geom + + + true + ${prefix}_l_finger_tip_link + 100.0 + ${prefix}_l_finger_tip_bumper + + + + + + + + + + + + + + + + + + true + false + + 100.0 + + ${prefix}_r_finger_tip_link_geom + + + true + ${prefix}_r_finger_tip_link + 100.0 + ${prefix}_r_finger_tip_bumper + + + + + + + + + + true + 100.0 + ${prefix}_l_finger_link + ${prefix}_l_finger_pose_ground_truth + 0.0 + base_link + + + true + 100.0 + ${prefix}_l_finger_link + ${prefix}_l_finger_force_ground_truth + ${prefix}_l_finger_link + + + + + + + + + + + + + + 0.17126 + + 7.7562e-05 + 1.49095e-06 + -9.83385e-06 + 0.000197083 + -3.06125e-06 + 0.000181054 + + 0.03598 0.0173 -0.00164 0 0 0 + + 0.82991 ${0.188*reflect+0.031} 0.790675 0 -0 0 + false + + + + 0.17389 + + 7.73841e-05 + -2.09309e-06 + -8.36228e-06 + 0.000198474 + 2.4611e-06 + 0.00018107 + + 0.03576 -0.01736 -0.00095 0 0 0 + + 0.82991 ${0.188*reflect-0.031} 0.790675 0 0 0 + false + + + + + ${prefix}_motor_screw_link + ${prefix}_r_finger_tip_link + -3141.6 + + 0 1 0 + + + + ${prefix}_l_finger_tip_link + ${prefix}_motor_screw_link + 3141.6 + + 0 1 0 + + + + + + + + ${prefix}_r_parallel_link + ${prefix}_palm_link + + 0 0 -1 + + 0.2 + + + ${0.07691+parallel_link_x_offset} ${-0.01-parallel_link_y_offset} 0 0 0 0 + + + ${prefix}_l_parallel_link + ${prefix}_palm_link + + 0 0 1 + + 0.2 + + + ${0.07691+parallel_link_x_offset} ${0.01+parallel_link_y_offset} 0 0 0 0 + + + ${prefix}_r_parallel_link + ${prefix}_r_finger_tip_link + + 0 0 1 + + ${parallel_link_x_offset} ${-parallel_link_y_offset} 0 0 0 0 + + + ${prefix}_l_parallel_link + ${prefix}_l_finger_tip_link + + 0 0 1 + + ${parallel_link_x_offset} ${parallel_link_y_offset} 0 0 0 0 + + + ${prefix}_r_finger_tip_link + ${prefix}_l_finger_tip_link + + 0 1 0 + + + + + true + + + + true + + + + true + + + + true + + + + + + + + + + + true + + + + + true + 100.0 + ${side}_gripper_palm_link + ${side}_gripper_palm_pose_ground_truth + 0 0 0 + 0 0 0 + 0.0 + map + + + + + + ${side}_gripper_r_finger_tip_link + ${side}_gripper_l_finger_tip_link + ${side}_gripper_palm_link + + + + + + + diff --git a/urdf/gripper_v0/gripper.transmission.xacro b/urdf/gripper_v0/gripper.transmission.xacro new file mode 100644 index 0000000..0486b74 --- /dev/null +++ b/urdf/gripper_v0/gripper.transmission.xacro @@ -0,0 +1,50 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/gripper_v0/gripper.urdf.xacro b/urdf/gripper_v0/gripper.urdf.xacro new file mode 100644 index 0000000..13085bb --- /dev/null +++ b/urdf/gripper_v0/gripper.urdf.xacro @@ -0,0 +1,374 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/head_v0/head.gazebo.xacro b/urdf/head_v0/head.gazebo.xacro new file mode 100644 index 0000000..7664455 --- /dev/null +++ b/urdf/head_v0/head.gazebo.xacro @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/urdf/head_v0/head.transmission.xacro b/urdf/head_v0/head.transmission.xacro new file mode 100644 index 0000000..b6e9356 --- /dev/null +++ b/urdf/head_v0/head.transmission.xacro @@ -0,0 +1,34 @@ + + + + + + + + + + + 6.0 + + + + + + + + + + + + 6.0 + + + + + + + + diff --git a/urdf/head_v0/head.urdf.xacro b/urdf/head_v0/head.urdf.xacro new file mode 100644 index 0000000..7e20545 --- /dev/null +++ b/urdf/head_v0/head.urdf.xacro @@ -0,0 +1,147 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/materials.urdf.xacro b/urdf/materials.urdf.xacro new file mode 100644 index 0000000..7c45090 --- /dev/null +++ b/urdf/materials.urdf.xacro @@ -0,0 +1,52 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/double_stereo_camera.gazebo.xacro b/urdf/sensors/double_stereo_camera.gazebo.xacro new file mode 100644 index 0000000..1a9d741 --- /dev/null +++ b/urdf/sensors/double_stereo_camera.gazebo.xacro @@ -0,0 +1,16 @@ + + + + + + + + + + + + + diff --git a/urdf/sensors/double_stereo_camera.urdf.xacro b/urdf/sensors/double_stereo_camera.urdf.xacro new file mode 100644 index 0000000..97910fe --- /dev/null +++ b/urdf/sensors/double_stereo_camera.urdf.xacro @@ -0,0 +1,61 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/head_sensor_package.gazebo.xacro b/urdf/sensors/head_sensor_package.gazebo.xacro new file mode 100644 index 0000000..f1b3b57 --- /dev/null +++ b/urdf/sensors/head_sensor_package.gazebo.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/head_sensor_package.urdf.xacro b/urdf/sensors/head_sensor_package.urdf.xacro new file mode 100644 index 0000000..b8b9fab --- /dev/null +++ b/urdf/sensors/head_sensor_package.urdf.xacro @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/hokuyo_lx30_laser.gazebo.xacro b/urdf/sensors/hokuyo_lx30_laser.gazebo.xacro new file mode 100644 index 0000000..e381385 --- /dev/null +++ b/urdf/sensors/hokuyo_lx30_laser.gazebo.xacro @@ -0,0 +1,38 @@ + + + + + + + + true + ${update_rate} + 0 0 0 0 0 0 + false + + + + 640 + 1 + ${min_angle} + ${max_angle} + + + + 0.08 + 10.0 + 0.01 + + + + 0.005 + true + ${update_rate} + ${ros_topic} + ${name}_link + + + + + + diff --git a/urdf/sensors/hokuyo_lx30_laser.urdf.xacro b/urdf/sensors/hokuyo_lx30_laser.urdf.xacro new file mode 100644 index 0000000..136a1d2 --- /dev/null +++ b/urdf/sensors/hokuyo_lx30_laser.urdf.xacro @@ -0,0 +1,27 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/kinect_camera.gazebo.xacro b/urdf/sensors/kinect_camera.gazebo.xacro new file mode 100644 index 0000000..daee077 --- /dev/null +++ b/urdf/sensors/kinect_camera.gazebo.xacro @@ -0,0 +1,75 @@ + + + + + + + true + 20.0 + + ${57.0*M_PI/180.0} + + R8G8B8 + 640 + 480 + + + 0.01 + 5 + + + + true + 20.0 + ${name} + ${name}/ir/image_raw + ${name}/ir/camera_info + ${name}/depth/image_raw + ${name}/depth/camera_info + ${name}/depth/points + ${name}_optical_frame + 0.5 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + true + 20.0 + + ${57.0*M_PI/180.0} + + R8G8B8 + 640 + 480 + + + 0.01 + 5 + + + + true + 20.0 + ${name} + ${name}/rgb/image_raw + ${name}/rgb/points + ${name}/rgb/camera_info + ${name}_optical_frame + 0.5 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + + + + + diff --git a/urdf/sensors/kinect_camera.urdf.xacro b/urdf/sensors/kinect_camera.urdf.xacro new file mode 100644 index 0000000..23b5b0c --- /dev/null +++ b/urdf/sensors/kinect_camera.urdf.xacro @@ -0,0 +1,55 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/kinect_prosilica_camera.gazebo.xacro b/urdf/sensors/kinect_prosilica_camera.gazebo.xacro new file mode 100644 index 0000000..df63036 --- /dev/null +++ b/urdf/sensors/kinect_prosilica_camera.gazebo.xacro @@ -0,0 +1,158 @@ + + + + + + + true + 1.0 + + ${57.0*M_PI/180.0} + + L8 + 640 + 480 + + + 0.01 + 5 + + + + 0.2 + true + 1.0 + ${camera_name}_ir + /${camera_name}/depth/image_raw + /${camera_name}/depth/camera_info + /${camera_name}/depth/image_raw + /${camera_name}/depth/camera_info + /${camera_name}/depth/points + ${frame_name} + 0.5 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + + + + + + + true + 1.0 + + ${57.0*M_PI/180.0} + + R8G8B8 + 640 + 480 + + + 0.01 + 5 + + + + true + 1.0 + ${camera_name}_rgb + /${camera_name}/rgb/image_raw + /${camera_name}/rgb/camera_info + /${camera_name}/depth_registered/points + ${frame_name} + 0.5 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + 0.00000001 + + + + + + + + + + ${link_name} + ${joint_name} + ${link_name} + + + + + + + 1 + + + + + + + true + 1.0 + + ${64.2*M_PI/180.0} + + R8G8B8 + 320 + 240 + + + 0.1 + 100 + + + + true + 1.0 + ${camera_name}_1mb + image_raw + camera_info + request_image + ${frame_name} + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + true + 1.0 + + ${64.2*M_PI/180.0} + + R8G8B8 + 320 + 240 + + + 0.1 + 100 + + + + true + 1.0 + ${camera_name}_1mb_sim_pcd + image_raw + camera_info + ${frame_name} + + + + + + + + diff --git a/urdf/sensors/kinect_prosilica_camera.urdf.xacro b/urdf/sensors/kinect_prosilica_camera.urdf.xacro new file mode 100644 index 0000000..0b597a3 --- /dev/null +++ b/urdf/sensors/kinect_prosilica_camera.urdf.xacro @@ -0,0 +1,181 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/microstrain_3dmgx2_imu.gazebo.xacro b/urdf/sensors/microstrain_3dmgx2_imu.gazebo.xacro new file mode 100644 index 0000000..fe5872e --- /dev/null +++ b/urdf/sensors/microstrain_3dmgx2_imu.gazebo.xacro @@ -0,0 +1,19 @@ + + + + + + + + true + ${update_rate} + ${name}_link + ${imu_topic} + ${stdev*stdev} + 0 0 0 + 0 -180.0 0 + + + + + diff --git a/urdf/sensors/microstrain_3dmgx2_imu.urdf.xacro b/urdf/sensors/microstrain_3dmgx2_imu.urdf.xacro new file mode 100644 index 0000000..952575f --- /dev/null +++ b/urdf/sensors/microstrain_3dmgx2_imu.urdf.xacro @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/projector_wg6802418.gazebo.xacro b/urdf/sensors/projector_wg6802418.gazebo.xacro new file mode 100644 index 0000000..ac32751 --- /dev/null +++ b/urdf/sensors/projector_wg6802418.gazebo.xacro @@ -0,0 +1,31 @@ + + + + + + + + 0 0 0 0 -1.5707 0 + stereo_projection_pattern_high_res_red.png + ${55.0/180.0*M_PI} + 0.1 + 10 + + + + + ${name}_child_frame/${name}_projector_wg6802418 + true + 15.0 + stereo_projection_pattern_high_res_red.png + stereo_projection_pattern_filter.png + ${name}_controller/image + ${name}_controller/projector + + + + + + diff --git a/urdf/sensors/projector_wg6802418.urdf.xacro b/urdf/sensors/projector_wg6802418.urdf.xacro new file mode 100644 index 0000000..9362aac --- /dev/null +++ b/urdf/sensors/projector_wg6802418.urdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/prosilica_gc2450_camera.gazebo.xacro b/urdf/sensors/prosilica_gc2450_camera.gazebo.xacro new file mode 100644 index 0000000..28d8475 --- /dev/null +++ b/urdf/sensors/prosilica_gc2450_camera.gazebo.xacro @@ -0,0 +1,43 @@ + + + + + + + true + 20.0 + + ${M_PI/4} + + R8G8B8 + 2448 + 2050 + + + 0.1 + 100 + + + + 20.0 + ${camera_name} + image_raw + camera_info + request_image + ${frame_name}_optical_frame + 1224.5 + 1224.5 + 1025.5 + 2955 + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + + + + + diff --git a/urdf/sensors/prosilica_gc2450_camera.urdf.xacro b/urdf/sensors/prosilica_gc2450_camera.urdf.xacro new file mode 100644 index 0000000..7f45621 --- /dev/null +++ b/urdf/sensors/prosilica_gc2450_camera.urdf.xacro @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/stereo_camera.gazebo.xacro b/urdf/sensors/stereo_camera.gazebo.xacro new file mode 100644 index 0000000..b53b8de --- /dev/null +++ b/urdf/sensors/stereo_camera.gazebo.xacro @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/stereo_camera.urdf.xacro b/urdf/sensors/stereo_camera.urdf.xacro new file mode 100644 index 0000000..a72102e --- /dev/null +++ b/urdf/sensors/stereo_camera.urdf.xacro @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/sensors/wge100_camera.gazebo.xacro b/urdf/sensors/wge100_camera.gazebo.xacro new file mode 100644 index 0000000..6dfad28 --- /dev/null +++ b/urdf/sensors/wge100_camera.gazebo.xacro @@ -0,0 +1,46 @@ + + + + + + true + 25.0 + + ${hfov*M_PI/180.0} + + ${image_format} + ${image_width} + ${image_height} + + + 0.1 + 100 + + + + true + 25.0 + ${camera_name} + ${image_topic_name} + ${camera_info_topic_name} + ${frame_id} + ${hack_baseline} + ${(image_width+1)/2} + ${(image_width+1)/2} + ${(image_height+1)/2} + + + ${focal_length} + 0.0 + 0.0 + 0.0 + 0.0 + 0.0 + + + true + PR2/Blue + + + + diff --git a/urdf/sensors/wge100_camera.urdf.xacro b/urdf/sensors/wge100_camera.urdf.xacro new file mode 100644 index 0000000..893a975 --- /dev/null +++ b/urdf/sensors/wge100_camera.urdf.xacro @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/shoulder_v0/shoulder.gazebo.xacro b/urdf/shoulder_v0/shoulder.gazebo.xacro new file mode 100644 index 0000000..58215d6 --- /dev/null +++ b/urdf/shoulder_v0/shoulder.gazebo.xacro @@ -0,0 +1,40 @@ + + + + + + + + true + + + + + + + + + true + + + + + + + + + + + + true + + + + + + + + diff --git a/urdf/shoulder_v0/shoulder.transmission.xacro b/urdf/shoulder_v0/shoulder.transmission.xacro new file mode 100644 index 0000000..c2ad1e4 --- /dev/null +++ b/urdf/shoulder_v0/shoulder.transmission.xacro @@ -0,0 +1,35 @@ + + + + + + + + + + ${13/3 * 250.532/17.19 / cal_r_shoulder_pan_gearing} + + + + + + + + ${13/3 * 245.532/17.19 / cal_r_shoulder_lift_gearing} + + + + + + + + + + + ${13/3 * 129.53/17.19 / cal_r_upper_arm_roll_gearing} + + + diff --git a/urdf/shoulder_v0/shoulder.urdf.xacro b/urdf/shoulder_v0/shoulder.urdf.xacro new file mode 100644 index 0000000..52fa5c1 --- /dev/null +++ b/urdf/shoulder_v0/shoulder.urdf.xacro @@ -0,0 +1,167 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/tilting_laser_v0/tilting_laser.gazebo.xacro b/urdf/tilting_laser_v0/tilting_laser.gazebo.xacro new file mode 100644 index 0000000..67939c9 --- /dev/null +++ b/urdf/tilting_laser_v0/tilting_laser.gazebo.xacro @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/urdf/tilting_laser_v0/tilting_laser.transmission.xacro b/urdf/tilting_laser_v0/tilting_laser.transmission.xacro new file mode 100644 index 0000000..75f814a --- /dev/null +++ b/urdf/tilting_laser_v0/tilting_laser.transmission.xacro @@ -0,0 +1,14 @@ + + + + + + + + -6.05 + + + diff --git a/urdf/tilting_laser_v0/tilting_laser.urdf.xacro b/urdf/tilting_laser_v0/tilting_laser.urdf.xacro new file mode 100644 index 0000000..89d8707 --- /dev/null +++ b/urdf/tilting_laser_v0/tilting_laser.urdf.xacro @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/torso_v0/torso.gazebo.xacro b/urdf/torso_v0/torso.gazebo.xacro new file mode 100644 index 0000000..96c1fbf --- /dev/null +++ b/urdf/torso_v0/torso.gazebo.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + ${name}_link_geom + + 100.0 + + true + 100.0 + ${name}_bumper + + + + + + + ${name}_link + ${name}_motor_screw_link + + 0 0 1 + + 3141.6 + + + + + + + + diff --git a/urdf/torso_v0/torso.transmission.xacro b/urdf/torso_v0/torso.transmission.xacro new file mode 100644 index 0000000..a191d8c --- /dev/null +++ b/urdf/torso_v0/torso.transmission.xacro @@ -0,0 +1,22 @@ + + + + + + + + + + -47641.53 + + + + + + + + diff --git a/urdf/torso_v0/torso.urdf.xacro b/urdf/torso_v0/torso.urdf.xacro new file mode 100644 index 0000000..83fdf76 --- /dev/null +++ b/urdf/torso_v0/torso.urdf.xacro @@ -0,0 +1,122 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/upper_arm_v0/upper_arm.gazebo.xacro b/urdf/upper_arm_v0/upper_arm.gazebo.xacro new file mode 100644 index 0000000..4d7f5cc --- /dev/null +++ b/urdf/upper_arm_v0/upper_arm.gazebo.xacro @@ -0,0 +1,39 @@ + + + + + + true + + + + + + + + -1.0 + + + + true + + + + + + + + + + + + true + + + + + + diff --git a/urdf/upper_arm_v0/upper_arm.transmission.xacro b/urdf/upper_arm_v0/upper_arm.transmission.xacro new file mode 100644 index 0000000..6031365 --- /dev/null +++ b/urdf/upper_arm_v0/upper_arm.transmission.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + ${-13/3 * 127.532/15.28} + + + + + + + + + ${-576/25 * 55/14} + + + + diff --git a/urdf/upper_arm_v0/upper_arm.urdf.xacro b/urdf/upper_arm_v0/upper_arm.urdf.xacro new file mode 100644 index 0000000..2008355 --- /dev/null +++ b/urdf/upper_arm_v0/upper_arm.urdf.xacro @@ -0,0 +1,173 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +