HeeroYui-deprecated/nao_dcm
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

0.0.1 Beta Version - Initial Release

Browse Source
This commit is contained in:
Konstantinos Chatzilygeroudis 2014-05-10 00:46:24 +03:00
commit 298b01361b
120 changed files with 8786 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
nao_dcm_driver-build
CMakeLists.txt.user

19
LICENSE.txt Normal file
View File

@ -0,0 +1,19 @@
Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived
from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT
SHALL THE COPYRIGHT 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.

97
README.md Normal file
View File

@ -0,0 +1,97 @@
#nao_dcm
##ROS Stack for Aldebaran's Nao Humanoid
- Goal is to **connect to the machine of Nao** and not the API provided by Aldebaran.
- Ιt makes use only of the **DCM and Memory Proxies**.
- Supports **V4.0 robots (H21 and H25 Body Types)**.
- **Written purely in C++** and uses the latest *C++ SDK (1.14.5)*.
Version
----
0.0.1 (Beta)
Requirements
-----------
nao_dcm requires several packages to be installed in order to work properly:
* [ROS] - ROS >= **Hydro** (should also work on the forthcoming *Indigo*)
* [NaoQi C++ SDK] - **Version 1.14.5**
* [ROS MoveIt!] - Used for motion planning
* [ROS Control] - **Version >=0.6.0**
* [Webots for Nao] - The best simulator so far [optional]
* [Gazebo] - Work in progress but with satisfactory results (**Version >= 2.2.2 alongside gazebo-ros-pkgs >= 2.3.4**) [optional]
* [Nao Robot] - A real working Nao is the best "simulator" you'll ever get!! **Version >= V4.0 and flashed OpenNao OS >= 1.14.5**
Basic Usage
--------------
###Bringup nao_dcm driver (remotely)
```sh
roslaunch nao_dcm_bringup nao_dcm_{BodyType}_remote.launch
```
{BodyType} is either **H21** or **H25**.
This will connect to Nao Robot, start the cameras and open up a simple dashboard for monitoring and basic control over Nao.
###MoveIt Demo
```sh
roslaunch nao_dcm_moveit_config moveit_planner_{BodyType}.launch
```
{BodyType} is either **H21** or **H25**.
*This requires that you have connected to the Robot/Simulation via nao_dcm driver* and will open up **rviz with the Navigation plugin configured** to give Nao trajectories to execute.
###Webots Simulation
Launch your **Webots for Nao Simulator** and then **follow the instructions above to bringup nao_dcm driver** (remotely). *On some versions of Webots, it is required that you move the head before you get camera feedback. Also, changing the camera resolution via dynamic reconfigure will not have any effect as Webots uses fixed resolution.*
###Gazebo Simulation
```sh
roslaunch nao_dcm_gazebo nao_dcm_gazebo_{BodyType}.launch
```
{BodyType} is either **H21** or **H25**.
**NOTE: **This is work in progress and may not work very well.
This will **launch Gazebo Simulator** and **trajectory controllers** to simulate the real robot. *This is somewhat equivalent as bringing up nao_dcm_driver (remotely)*. Thus, then for example you can use MoveIt Demo to give trajectories to Nao to execute.
Notes/Limitations
-----------------
* **Before using the nao_dcm driver on your real Nao** *(because DCM needs calibration but Aldebaran does not provide enough documentation on how to calibrate it)*, it is recommended that you connect to your Nao (e.g. via choreographe/motion proxy) and increase the stiffnesses of the joints.
* Currently **nao_dcm can only be run remotely**, *as Nao Robot only supports ROS Groovy locally* and *nao_dcm requires ros_controls packages* (available only on >= Hydro). Yet, **nao_dcm_robot** folder is made to be run locally, so **when ROS Hydro support for Nao locally is available users can use it directly.**
* Tutorials will become available as soon as possible.
* Integration with [nao_meshes] for better visual feedback is in progress.
* Efforts for making ROS Hydro available locally on Nao, because only this way nao_dcm will get its maximum performance. **Help is welcomed.**
* Although my intension is to provide ROS integration to the machine, basic gait and motion planning schemes are on the way for those that want quick results/feedback.
* *Integration for LED, IR and Audio hardware is not available and is not on my agenda*. So, **feel free to contribute in that direction**.
* **This is only ONE part of my Diploma Thesis** (*"Navigation of Humanoid Robots in Unknown Space With Dynamic Obstacles"*), so many parts of the code are not perfect and well-thought. E.g. *initialize* method of Nao class. So, **Pull-Requests for code improvement are welcomed.**
Origin of the Name
------------------
Since my intention is to use the lowest level of Aldebaran's API possible and its name is DCM, I decided to name the ROS Stack nao_dcm.
License
----
BSD
Copyright (c) 2014, **Konstantinos Chatzilygeroudis**
[ros]: http://www.ros.org
[naoqi c++ sdk]: https://community.aldebaran-robotics.com/doc/1-14/index.html
[webots for nao]: https://community.aldebaran-robotics.com/doc/1-14/software/webots/webots_index.html
[gazebo]: http://gazebosim.org/
[ros moveit!]: http://moveit.ros.org/
[nao robot]: http://www.aldebaran.com/en/humanoid-robot/nao-robot
[nao_meshes]: https://github.com/vrabaud/nao_meshes
[ros control]: http://wiki.ros.org/ros_control

4
nao_dcm/CMakeLists.txt Normal file
View File

@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm)
find_package(catkin REQUIRED)
catkin_metapackage()

44
nao_dcm/package.xml Normal file
View File

@ -0,0 +1,44 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm</name>
<version>0.0.1</version>
<description>nao_dcm metapackage contains packages to integrate into ROS Aldebaran's Nao robot (v4).
This metapackage integrates the underlying machine of Nao and not the API provided by Aldebaran.
</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>nao_dcm_description</run_depend>
<run_depend>nao_dcm_driver</run_depend>
<run_depend>nao_dcm_msgs</run_depend>
<run_depend>nao_dcm_rqt_dashboard</run_depend>
<run_depend>nao_dcm_control</run_depend>
<run_depend>nao_dcm_gazebo</run_depend>
<run_depend>nao_dcm_moveit_config</run_depend>
<export>
<metapackage/>
</export>
</package>

58
nao_dcm_apps/nao_dcm_camera/CMakeLists.txt Normal file
View File

@ -0,0 +1,58 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm_camera)
# Tell CMake where to find "FindNAOqi.cmake"
list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/cmake")
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
std_msgs
image_transport
camera_info_manager
dynamic_reconfigure
nao_dcm_msgs
sensor_msgs
)
find_package(NAOqi QUIET COMPONENTS
alcommon
alvision
alproxies
alerror
alvalue
)
find_package(Boost)
generate_dynamic_reconfigure_options(cfg/NaoDCMCamera.cfg)
##Needed for ros packages
catkin_package(CATKIN_DEPENDS roscpp std_msgs nao_dcm_msgs sensor_msgs camera_info_manager dynamic_reconfigure)
if( NAOqi_FOUND AND Boost_FOUND)
message(STATUS "NAOqi C++ SDK found; C++ nodes will be built")
include_directories( include
${catkin_INCLUDE_DIRS}
${NAOqi_INCLUDE_DIRS}
${Boost_INCLUDE_DIRS})
add_executable(nao_dcm_camera src/nao_camera.cpp)
target_link_libraries(nao_dcm_camera
${catkin_LIBRARIES}
${NAOqi_LIBRARIES}
${Boost_LIBRARIES})
add_dependencies(nao_dcm_camera ${catkin_EXPORTED_TARGETS})
else()
message(STATUS "Cannot find NAOqi C++ sdk; C++ nodes will NOT be built")
endif()
install(TARGETS nao_dcm_camera
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY share
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

50
nao_dcm_apps/nao_dcm_camera/cfg/NaoDCMCamera.cfg Executable file
View File

@ -0,0 +1,50 @@
#!/usr/bin/env python
# Copyright (c) 2014, Konstantinos Chatzilygeroudis
# All rights reserved.
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
# 2. 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.
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
PACKAGE = "nao_dcm_camera"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# Name, Type, Reconfiguration level, Description, Default, Min, Max
resolutions = gen.enum([gen.const("160x120", int_t, 0, ""),
gen.const("320x240", int_t, 1, ""),
gen.const("640x480", int_t, 2, ""),
gen.const("1280x960", int_t, 3, "")
],
"Resolutions for camera.")
gen.add("top_resolution", int_t, 0,
"Camera Top resolution.", 1, edit_method = resolutions)
gen.add("bottom_resolution", int_t, 0,
"Camera Bottom resolution.", 1, edit_method = resolutions)
gen.add("top_frame_rate", double_t, 0,
"Camera Top speed (frames per second).", 30.0, 1., 30.0)
gen.add("bottom_frame_rate", double_t, 0,
"Camera Bottom speed (frames per second).", 30.0, 1., 30.0)
#gen.add("top_camera_info_url", str_t, 0,
# "Camera Top calibration file (camera_top_rgb8_[top_resolution]) for this video_mode (uncalibrated if null).", "")
#gen.add("bottom_camera_info_url", str_t, 0,
# "Camera Bottom calibration file (camera_bottom_rgb8_[top_resolution]) for this video_mode (uncalibrated if null).", "")
exit(gen.generate(PACKAGE, "NaoDCMCamera", "NaoDCMCamera"))

149
nao_dcm_apps/nao_dcm_camera/cmake/FindNAOqi.cmake Normal file
View File

@ -0,0 +1,149 @@
# Copyright (c) 2013, Miguel Sarabia
# Imperial College London
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# # Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# # Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
# # Neither the name of the Imperial College London nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# - Try to find NAOqi
# Once executed this script will define the following:
# NAOqi_FOUND - NAOqi was succesfully found
# NAOqi_INCLUDE_DIRS - NAOqi's include directories
# NAOqi_LIBRARIES - NAOqi's libraries
# NAOqi_DIR - Directory where NAOqi was found
#------------------------------------------------------------------------------
# Users can set NAOqi_DIR to force CMake to look in a particular location,
# setting the AL_DIR environment variable will have a similar effect.
cmake_minimum_required(VERSION 2.8.3)
#These are NAOqi's known components (ie. libraries)
set(NAOqi_COMPONENTS
alaudio
albonjourdiscovery
alextractor
allog
almodelutils
alproject
alresource
altools
alautomatictest
alboxrary
alfile
almathinternal
almotion
alpythonbridge
alserial
altts
albehaviorinfo
alcommon
allauncher
almath
almotionrecorder
alpythontools
alsoap
alvalue
albehavior
alerror
allogremote
almemoryfastaccess
alparammanager
alremotecall
althread
alvision
alproxies)
#Set INCLUDE hints
set(NAOqi_INCLUDE_HINTS
"${NAOqi_DIR}/include"
"$ENV{AL_DIR}/include" )
# Set LIBRARY hints
set(NAOqi_LIBRARY_HINTS
"${NAOqi_DIR}/lib"
"$ENV{AL_DIR}/lib" )
# Find include directories
find_path(NAOqi_INCLUDE_DIR alcommon/alproxy.h HINTS ${NAOqi_INCLUDE_HINTS} )
# Verify we know about all the components requested
# and remove those we don't know about
set(NAOqi_FILTERED_COMPONENTS ${NAOqi_FIND_COMPONENTS})
if ( NAOqi_FIND_COMPONENTS )
foreach(comp ${NAOqi_FIND_COMPONENTS})
list(FIND NAOqi_COMPONENTS ${comp} ${comp}_KNOWN)
if (${comp}_KNOWN EQUAL -1)
list(REMOVE_ITEM NAOqi_FILTERED_COMPONENTS ${comp})
message(STATUS "Unknown NAOqi component ${comp}")
endif()
endforeach()
endif()
list(LENGTH NAOqi_FILTERED_COMPONENTS NAOqi_NUMBER_OF_COMPONENTS)
set(NAOqi_FOUND_COMPONENTS TRUE)
# Look for components (ie. libraries)
if( ${NAOqi_NUMBER_OF_COMPONENTS} )
foreach(comp ${NAOqi_FILTERED_COMPONENTS})
#Look for the actual library here
find_library(${comp}_LIBRARY NAMES ${comp} HINTS ${NAOqi_LIBRARY_HINTS})
if ( ${${comp}_LIBRARY} STREQUAL ${comp}_LIBRARY-NOTFOUND)
message(STATUS "Could not find NAOqi's ${comp}")
set(NAOqi_FOUND_COMPONENTS FALSE)
else()
#If everything went well append this component to list of libraries
list(APPEND NAOqi_LIBRARY ${${comp}_LIBRARY})
endif()
endforeach()
else()
message(STATUS "No NAOqi components specified")
endif()
# Handle the QUIET and REQUIRED arguments
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(
NAOqi #Package name
DEFAULT_MSG
# Variables required to evaluate as TRUE
NAOqi_LIBRARY
NAOqi_INCLUDE_DIR
NAOqi_FOUND_COMPONENTS)
# Copy the values of the advanced variables to the user-facing ones
set(NAOqi_LIBRARIES ${NAOqi_LIBRARY} )
set(NAOqi_INCLUDE_DIRS ${NAOqi_INCLUDE_DIR} )
set(NAOqi_FOUND ${NAOQI_FOUND})
# If NAOqi was found, update NAOqi_DIR to show where it was found
if ( NAOqi_FOUND )
get_filename_component(NAOqi_NEW_DIR "${NAOqi_INCLUDE_DIRS}/../" ABSOLUTE)
endif()
set(NAOqi_DIR ${NAOqi_NEW_DIR} CACHE FILEPATH "NAOqi root directory" FORCE)
#Hide these variables
mark_as_advanced(NAOqi_INCLUDE_DIR NAOqi_LIBRARY NAOQI_FOUND)

124
nao_dcm_apps/nao_dcm_camera/include/nao_dcm_camera/nao_camera.h Normal file
View File

@ -0,0 +1,124 @@
/**Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
**/
#ifndef NAO_CAMERA_H
#define NAO_CAMERA_H
// Boost Headers
#include <boost/shared_ptr.hpp>
#include <boost/assign.hpp>
#include <boost/thread/mutex.hpp>
// NAOqi Headers
#include <alcommon/almodule.h>
#include <alproxies/alvideodeviceproxy.h>
#include <alvision/alimage.h>
#include <qi/os.hpp>
// ROS Headers
#include <ros/ros.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <nao_dcm_msgs/BoolService.h>
#include <image_transport/image_transport.h>
#include <image_transport/camera_publisher.h>
#include <camera_info_manager/camera_info_manager.h>
#include <dynamic_reconfigure/server.h>
#include <nao_dcm_camera/NaoDCMCameraConfig.h>
using std::string;
using std::cerr;
using std::endl;
namespace AL
{
class ALBroker;
}
class NaoCamera : public AL::ALModule
{
private:
// ROS Standard Variables
ros::NodeHandle node_handle_, top_node_handle_, bottom_node_handle_;
// ROS Topics/Messages
image_transport::CameraPublisher camera_top_image_pub_, camera_bottom_image_pub_;
sensor_msgs::CameraInfo camera_top_, camera_bottom_;
sensor_msgs::Image camera_top_image_, camera_bottom_image_;
ros::ServiceServer camera_top_switch_, camera_bottom_switch_;
boost::shared_ptr<camera_info_manager::CameraInfoManager> camera_top_info_, camera_bottom_info_;
// ROS Dynamic Reconfigure
dynamic_reconfigure::Server<nao_dcm_camera::NaoDCMCameraConfig> dynamic_req_server_;
nao_dcm_camera::NaoDCMCameraConfig cameras_config_;
volatile bool reconfiguring_;
// Member Variables
// Camera Specific
bool camera_top_enabled_, camera_bottom_enabled_, camera_enabled_;
// Helper
bool is_connected_;
int topic_queue_;
string prefix_;
// AL Proxies
AL::ALVideoDeviceProxy video_proxy_;
public:
// Constructor/Destructor
NaoCamera(boost::shared_ptr<AL::ALBroker> broker, const std::string& name);
~NaoCamera();
bool initialize();
// Connect/Disconnet to ALProxies
bool connect(const ros::NodeHandle nh, const ros::NodeHandle top, const ros::NodeHandle bottom);
void disconnect();
// Subscribe to ROS Topics/Services
void subscribe();
// Parameter Server
void loadParams();
// Helper
void brokerDisconnected(const string& event_name, const string &broker_name, const string& subscriber_identifier);
string getURLFromConfiguration(const string& camera, const int& resolution, const int &color = 0);
// ALVideoDevice Methods
AL::ALValue getImage(const string &cam_module="NaoCameraTop");
// General Methods
void loop(const ros::Time& time, const ros::Duration& period);
bool connected();
// ROS Callbacks/Related Methods
void publishCameras(const ros::Time &ts);
bool switchCameraTopState(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res);
bool switchCameraBottomState(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res);
void dynamicReconfigureCb(nao_dcm_camera::NaoDCMCameraConfig &config, uint32_t level);
};
#endif // NAO_H

52
nao_dcm_apps/nao_dcm_camera/package.xml Normal file
View File

@ -0,0 +1,52 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm_camera</name>
<version>0.0.1</version>
<description>Package containing the camera driver to connect to Aldebaran's Nao robot's cameras.</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>camera_info_manager</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>nao_dcm_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>camera_info_manager</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>nao_dcm_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<export>
</export>
</package>

20
nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_1280x960.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 1280
image_height: 960
camera_name: NaoCameraBottom
camera_matrix:
rows: 3
cols: 3
data: [1219.448171, 0, 614.481756, 0, 1291.421024, 151.265905, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.005928, 0.118999, -0.081849, -0.003667, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [1249.13147, 0, 611.294675, 0, 0, 1188.448242, 108.11769, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_160x120.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 160
image_height: 120
camera_name: NaoCameraBottom
camera_matrix:
rows: 3
cols: 3
data: [155.224589, 0, 80.709808, 0, 158.41574, 25.169391, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.0771, 0.160121, -0.038934, 0.004482, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [154.612289, 0, 80.756829, 0, 0, 151.163651, 22.228715, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_320x240.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 320
image_height: 240
camera_name: NaoCameraBottom
camera_matrix:
rows: 3
cols: 3
data: [321.873789, 0, 157.06588, 0, 328.583553, 51.991062, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.058408, 0.681358, -0.055512, -0.004355, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [349.489349, 0, 156.09221, 0, 0, 346.640076, 47.94287, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_bottom_rgb8_640x480.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 640
image_height: 480
camera_name: NaoCameraBottom
camera_matrix:
rows: 3
cols: 3
data: [581.996595, 0, 313.488982, 0, 583.026436, 210.571778, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.035206, 0.08788, 0.002187, -0.001672, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [584.987427, 0, 312.169289, 0, 0, 584.710754, 211.522273, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_1280x960.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 1280
image_height: 960
camera_name: NaoCameraTop
camera_matrix:
rows: 3
cols: 3
data: [1248.631984, 0, 559.742668, 0, 1260.921559, 349.814455, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.02015, 0.030736, -0.02731, -0.011039, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [1255.832153, 0, 547.89151, 0, 0, 1245.243896, 329.706716, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_160x120.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 160
image_height: 120
camera_name: NaoCameraTop
camera_matrix:
rows: 3
cols: 3
data: [149.968982, 0, 80.31209, 0, 150.997704, 48.927609, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.082615, 0.173998, -0.021707, -0.006771, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [150.092209, 0, 78.890158, 0, 0, 149.09726, 46.450828, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_320x240.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 320
image_height: 240
camera_name: NaoCameraTop
camera_matrix:
rows: 3
cols: 3
data: [294.056719, 0, 156.161902, 0, 298.870595, 61.772464, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [0.002381, -0.079306, -0.036802, -0.004818, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [294.832886, 0, 154.23345, 0, 0, 282.856323, 54.554838, 0, 0, 0, 1, 0]

20
nao_dcm_apps/nao_dcm_camera/share/camera_top_rgb8_640x480.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 640
image_height: 480
camera_name: NaoCameraTop
camera_matrix:
rows: 3
cols: 3
data: [554.903478, 0, 306.432389, 0, 553.554369, 208.613332, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.09743, 0.125595, -0.016384, -0.008456, 0]
rectification_matrix:
rows: 3
cols: 3
data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
projection_matrix:
rows: 3
cols: 4
data: [550.039795, 0, 300.903728, 0, 0, 544.60376, 200.810548, 0, 0, 0, 1, 0]

444
nao_dcm_apps/nao_dcm_camera/src/nao_camera.cpp Normal file
View File

@ -0,0 +1,444 @@
/**Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
**/
#include <iostream>
#include "nao_dcm_camera/nao_camera.h"
#include <alcommon/albroker.h>
#include <alcommon/albrokermanager.h>
using std::vector;
NaoCamera::NaoCamera(boost::shared_ptr<AL::ALBroker> broker, const string &name)
: AL::ALModule(broker,name),is_connected_(false),reconfiguring_(false)
{
functionName("brokerDisconnected", getName(), "Callback when broker disconnects!");
BIND_METHOD(NaoCamera::brokerDisconnected);
}
NaoCamera::~NaoCamera()
{
if(is_connected_)
disconnect();
}
bool NaoCamera::initialize()
{
dynamic_req_server_.setCallback(boost::bind(&NaoCamera::dynamicReconfigureCb, this, _1, _2));
}
bool NaoCamera::connect(const ros::NodeHandle nh, const ros::NodeHandle top, const ros::NodeHandle bottom)
{
node_handle_ = nh;
top_node_handle_ = top;
bottom_node_handle_ = bottom;
is_connected_ = false;
// Loada parameter server parameters
loadParams();
// If camera not enabled, skip initialization
if(!camera_enabled_)
{
ROS_WARN("Cameras not enabled! Shutting down NaoCamera node..");
is_connected_ = true;
return true;
}
try
{
video_proxy_ = AL::ALVideoDeviceProxy(getParentBroker());
if(camera_top_enabled_)
{
video_proxy_.subscribeCamera("NaoCameraTop", 0, 1, 11, 30);
video_proxy_.setActiveCamera("NaoCameraTop",0);
}
if(camera_bottom_enabled_)
{
video_proxy_.subscribeCamera("NaoCameraBottom", 1, 1, 11, 30);
video_proxy_.setActiveCamera("NaoCameraBottom",1);
}
}
catch (const AL::ALError& e)
{
ROS_ERROR("Failed to connect to Camera Proxy!\n\tTrace: %s",e.what());
return false;
}
// Initialize Cameras Info
camera_top_info_ = boost::shared_ptr<camera_info_manager::CameraInfoManager>
(new camera_info_manager::CameraInfoManager(top_node_handle_));
camera_top_info_->setCameraName("NaoCameraTop");
camera_bottom_info_ = boost::shared_ptr<camera_info_manager::CameraInfoManager>
(new camera_info_manager::CameraInfoManager(bottom_node_handle_));
camera_bottom_info_->setCameraName("NaoCameraBottom");
// Load Default Camera Configurations
cameras_config_.top_frame_rate = video_proxy_.getFrameRate(0);
cameras_config_.top_resolution = video_proxy_.getResolution(0);
cameras_config_.bottom_frame_rate = video_proxy_.getFrameRate(1);
cameras_config_.top_resolution = video_proxy_.getResolution(1);
camera_top_info_->loadCameraInfo(getURLFromConfiguration("top",cameras_config_.top_resolution));
camera_bottom_info_->loadCameraInfo(getURLFromConfiguration("bottom",cameras_config_.bottom_resolution));
is_connected_ = true;
subscribe();
if(!initialize())
return false;
ROS_INFO("NaoCamera Module initialized!");
return true;
}
void NaoCamera::disconnect()
{
if(!is_connected_)
return;
try
{
video_proxy_.unsubscribe("NaoCameraTop");
video_proxy_.unsubscribe("NaoCameraBottom");
}
catch (const AL::ALError& e)
{
ROS_ERROR("Failed to unsubscribe from cameras!\n\tTrace: %s",e.what());
}
is_connected_ = false;
}
void NaoCamera::loadParams()
{
node_handle_.param("Nao_UseCamera", camera_enabled_, true);
node_handle_.param("Nao_TopCameraEnabled", camera_top_enabled_, camera_enabled_);
node_handle_.param("Nao_BottomCameraEnabled", camera_bottom_enabled_, camera_enabled_);
node_handle_.param("Nao_TopicQueue", topic_queue_, 50);
node_handle_.param("Nao_Prefix", prefix_, string("nao_dcm"));
prefix_ = prefix_+"/";
}
void NaoCamera::brokerDisconnected(const string& event_name, const string &broker_name,
const string& subscriber_identifier)
{
if(broker_name == "Nao Camera Broker")
is_connected_ = false;
}
string NaoCamera::getURLFromConfiguration(const string &camera, const int &resolution, const int &color)
{
// Color is not used as we allow only for rgb8 mode of the cameras (we could provide more in the future)
string res = "640x480";
switch (resolution) {
case 0:
res = "160x120";
break;
case 1:
res = "320x240";
break;
case 2:
res = "640x480";
break;
case 3:
res = "1280x960";
break;
default:
res = "640x480";
break;
}
return "package://nao_dcm_camera/share/camera_"+camera+"_rgb8_"+res+".yaml";
}
void NaoCamera::subscribe()
{
image_transport::ImageTransport it(node_handle_);
camera_top_image_pub_ = it.advertiseCamera(prefix_+"CameraTop/image_raw", topic_queue_);
camera_bottom_image_pub_ = it.advertiseCamera(prefix_+"CameraBottom/image_raw", topic_queue_);
camera_top_switch_ = node_handle_.advertiseService<NaoCamera, nao_dcm_msgs::BoolService::Request,
nao_dcm_msgs::BoolService::Response>(prefix_+"CameraTop/Enable", &NaoCamera::switchCameraTopState, this);
camera_bottom_switch_ = node_handle_.advertiseService<NaoCamera, nao_dcm_msgs::BoolService::Request,
nao_dcm_msgs::BoolService::Response>(prefix_+"CameraBottom/Enable",
&NaoCamera::switchCameraBottomState, this);
}
AL::ALValue NaoCamera::getImage(const string& cam_module)
{
AL::ALValue results;
bool err=false;
try
{
results = video_proxy_.getImageRemote(cam_module);
}
catch(const AL::ALError& e)
{
err=true;
ROS_ERROR("Could not get/write image.\n\tTrace: %s",e.what());
}
if(!err)
{
try
{
video_proxy_.releaseImage(cam_module);
}
catch(const AL::ALError& e)
{
ROS_ERROR("Could not release image.\n\tTrace: %s",e.what());
}
}
return results;
}
void NaoCamera::loop(const ros::Time &time, const ros::Duration &period)
{
if(!camera_enabled_)
{
is_connected_ = false;
return;
}
if(reconfiguring_)
return;
try
{
publishCameras(time);
}
catch(ros::Exception& e)
{
ROS_WARN("NaoCamera Error: %s",e.what());
}
catch(AL::ALError& e)
{
ROS_ERROR("ALError!\n\tTrace: %s",e.what());
}
}
bool NaoCamera::connected()
{
return is_connected_;
}
void NaoCamera::publishCameras(const ros::Time &ts)
{
if(camera_top_enabled_)
{
AL::ALValue im_info = getImage("NaoCameraTop");
if(im_info.getSize()!=12)
{
return;
}
const unsigned char* im = static_cast<const unsigned char*>(im_info[6].GetBinary());
if(im==NULL)
return;
int w, h, nbLayers;
w = (int)im_info[0];
h = (int)im_info[1];
nbLayers = (int)im_info[2];
vector<unsigned char> v(im, (im+w*h*nbLayers));
camera_top_.header.frame_id = "CameraTop";
camera_top_.header.stamp = ts;
camera_top_ = camera_top_info_->getCameraInfo();
camera_top_.header.frame_id = "CameraTop";
camera_top_.header.stamp = ts;
camera_top_image_.header.frame_id = "CameraTop";
camera_top_image_.header.stamp = ts;
camera_top_image_.data = v;
camera_top_image_.width = w;
camera_top_image_.height = h;
camera_top_image_.step = w*nbLayers;
// TO-DO: Need to find encoding from Nao Image
camera_top_image_.encoding = "rgb8";
camera_top_image_pub_.publish(camera_top_image_, camera_top_);
}
if(camera_bottom_enabled_)
{
AL::ALValue im_info = getImage("NaoCameraBottom");
if(im_info.getSize()!=12)
{
return;
}
const unsigned char* im = static_cast<const unsigned char*>(im_info[6].GetBinary());
if(im==NULL)
return;
int w = (int)im_info[0];
int h = (int)im_info[1];
int nbLayers = (int)im_info[2];
vector<unsigned char> v = vector<unsigned char> (im, (im+w*h*nbLayers));
camera_bottom_.header.frame_id = "CameraBottom";
camera_bottom_.header.stamp = ts;
camera_bottom_ = camera_bottom_info_->getCameraInfo();
camera_bottom_.header.frame_id = "CameraBottom";
camera_bottom_.header.stamp = ts;
camera_bottom_image_.header.frame_id = "CameraBottom";
camera_bottom_image_.header.stamp = ts;
camera_bottom_image_.data = v;
camera_bottom_image_.width = w;
camera_bottom_image_.height = h;
camera_bottom_image_.step = w*nbLayers;
// TO-DO: Need to find encoding from Nao Image
camera_bottom_image_.encoding = "rgb8";
camera_bottom_image_pub_.publish(camera_bottom_image_, camera_bottom_);
}
}
bool NaoCamera::switchCameraTopState(nao_dcm_msgs::BoolService::Request &req, nao_dcm_msgs::BoolService::Response &res)
{
if(!camera_enabled_)
return true;
camera_top_enabled_ = req.enable;
return true;
}
bool NaoCamera::switchCameraBottomState(nao_dcm_msgs::BoolService::Request &req,
nao_dcm_msgs::BoolService::Response &res)
{
if(!camera_enabled_)
return true;
camera_bottom_enabled_ = req.enable;
return true;
}
void NaoCamera::dynamicReconfigureCb(nao_dcm_camera::NaoDCMCameraConfig &config, uint32_t level)
{
reconfiguring_ = true;
// Change configurations if needed
if(camera_top_enabled_)
{
if(cameras_config_.top_resolution != config.top_resolution)
{
video_proxy_.setResolution("NaoCameraTop", config.top_resolution);
camera_top_info_->loadCameraInfo(getURLFromConfiguration("top", config.top_resolution));
}
if(cameras_config_.top_frame_rate != config.top_frame_rate)
{
video_proxy_.setFrameRate("NaoCameraTop", config.top_frame_rate);
}
}
if(camera_bottom_enabled_)
{
if(cameras_config_.bottom_resolution != config.bottom_resolution)
{
video_proxy_.setResolution("NaoCameraBottom", config.bottom_resolution);
camera_bottom_info_->loadCameraInfo(getURLFromConfiguration("bottom",config.bottom_resolution));
}
if(cameras_config_.bottom_frame_rate != config.bottom_frame_rate)
{
video_proxy_.setFrameRate("NaoCameraBottom", config.bottom_frame_rate);
}
}
// Store new configurations
cameras_config_ = config;
reconfiguring_ = false;
}
int main( int argc, char** argv )
{
int pport = 9559;
string pip = "127.0.0.1";
ros::init(argc, argv, "nao_dcm_camera");
ros::NodeHandle n;
ros::NodeHandle top("CameraTopNode"), bottom("CameraBottomNode");
if(!ros::master::check())
{
cerr<<"Could not contact master!\nQuitting... "<<endl;
return -1;
}
// A broker needs a name, an IP and a port:
string broker_name = "Nao Camera Broker";
// FIXME: would be a good idea to look for a free port first
int broker_port = 54001;
// listen port of the broker (here an anything)
string broker_ip = "0.0.0.0";
double communication_rate;
// Load Params from Parameter Server
n.param("Nao_RobotIP", pip, string("127.0.0.1"));
n.param("Nao_RobotPort", pport,9559);
n.param("Nao_CameraBrokerPort", broker_port, 54001);
n.param("Nao_CameraBrokerIP", broker_ip, string("0.0.0.0"));
n.param("Nao_CameraFrequency", communication_rate, 30.0);
// Create your own broker
boost::shared_ptr<AL::ALBroker> broker;
try
{
broker = AL::ALBroker::createBroker(broker_name,broker_ip,broker_port,pip,pport,0);
}
catch(...)
{
ROS_ERROR("Failed to connect to Broker at %s:%d!",pip.c_str(),pport);
AL::ALBrokerManager::getInstance()->removeBroker(broker);
return -1;
}
// Deal with ALBrokerManager singleton (add your broker into NAOqi)
AL::ALBrokerManager::setInstance(broker->fBrokerManager.lock());
AL::ALBrokerManager::getInstance()->addBroker(broker);
// Now it's time to load your module
boost::shared_ptr<NaoCamera> nao_cam = AL::ALModule::createModule<NaoCamera>(broker, "NaoCamera");
nao_cam->connect(n,top,bottom);
if(!nao_cam->connected())
{
ROS_ERROR("Could not connect to Nao robot!");
AL::ALBrokerManager::getInstance()->killAllBroker();
AL::ALBrokerManager::kill();
return -1;
}
ros::Rate rate(communication_rate);
// Run the spinner in a separate thread to prevent lockups
ros::AsyncSpinner spinner(1);
spinner.start();
if(broker->isModulePresent("NaoCamera"))
ROS_INFO("NaoCamera Module loaded succesfully!");
else
{
ROS_ERROR("NaoCamera Module is not loaded!");
return -1;
}
while(ros::ok())
{
nao_cam->loop(ros::Time::now(), rate.expectedCycleTime());
if(!nao_cam->connected())
{
ROS_WARN("NaoCamera unexpectedly disconnected!");
break;
}
rate.sleep();
}
AL::ALBrokerManager::getInstance()->killAllBroker();
AL::ALBrokerManager::kill();
spinner.stop();
ROS_INFO( "Quitting... " );
return 0;
}

17
nao_dcm_apps/nao_dcm_gazebo/CMakeLists.txt Normal file
View File

@ -0,0 +1,17 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm_gazebo)
## Find catkin
find_package(catkin REQUIRED COMPONENTS gazebo_ros)
##Needed for ros packages
catkin_package()
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY worlds
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

52
nao_dcm_apps/nao_dcm_gazebo/config/gazebo_ros_control_params.yaml Normal file
View File

@ -0,0 +1,52 @@
nao_dcm/gazebo_ros_control/pid_gains:
HeadYaw: {p: 60.0, i: 80.0, d: 0.3}
HeadPitch: {p: 60.0, i: 80.0, d: 0.3}
LShoulderPitch: {p: 60.0, i: 80.0, d: 0.3}
LElbowYaw: {p: 60.0, i: 80.0, d: 0.3}
LElbowRoll: {p: 60.0, i: 80.0, d: 0.3}
LShoulderRoll: {p: 60.0, i: 80.0, d: 0.3}
LWristYaw: {p: 60.0, i: 80.0, d: 0.3}
LHand: {p: 60.0, i: 80.0, d: 0.3}
RShoulderPitch: {p: 60.0, i: 80.0, d: 0.3}
RElbowYaw: {p: 60.0, i: 80.0, d: 0.3}
RElbowRoll: {p: 60.0, i: 80.0, d: 0.3}
RShoulderRoll: {p: 60.0, i: 80.0, d: 0.3}
RWristYaw: {p: 60.0, i: 80.0, d: 0.3}
RHand: {p: 60.0, i: 80.0, d: 0.3}
LHipYawPitch: {p: 60.0, i: 80.0, d: 0.3}
LHipRoll: {p: 60.0, i: 80.0, d: 0.3}
LHipPitch: {p: 60.0, i: 80.0, d: 0.3}
LKneePitch: {p: 60.0, i: 80.0, d: 0.3}
LAnklePitch: {p: 60.0, i: 80.0, d: 0.3}
LAnkleRoll: {p: 60.0, i: 80.0, d: 0.3}
RHipYawPitch: {p: 60.0, i: 80.0, d: 0.3}
RHipRoll: {p: 60.0, i: 80.0, d: 0.3}
RHipPitch: {p: 60.0, i: 80.0, d: 0.3}
RKneePitch: {p: 60.0, i: 80.0, d: 0.3}
RAnklePitch: {p: 60.0, i: 80.0, d: 0.3}
RAnkleRoll: {p: 60.0, i: 80.0, d: 0.3}

18
nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H21.launch Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<launch>
<!-- Load the URDF Model -->
<include file="$(find nao_dcm_description)/launch/nao_dcm_H21_urdf.launch" />
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find nao_dcm_gazebo)/worlds/nao_test.world"/>
<arg name="paused" value="true"/>
<!-- more default parameters can be changed here -->
</include>
<rosparam file="$(find nao_dcm_gazebo)/config/gazebo_ros_control_params.yaml" command="load"/>
<!-- Call Nao Robot Trajectory Controller -->
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H21.launch"/>
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -x 0 -y 0 -z 0.339 -model nao_robot_v4_H21" />
</launch>

18
nao_dcm_apps/nao_dcm_gazebo/launch/nao_dcm_gazebo_H25.launch Normal file
View File

@ -0,0 +1,18 @@
<?xml version="1.0"?>
<launch>
<!-- Load the URDF Model -->
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
<!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" value="$(find nao_dcm_gazebo)/worlds/nao_test.world"/>
<arg name="paused" value="true"/>
<!-- more default parameters can be changed here -->
</include>
<rosparam file="$(find nao_dcm_gazebo)/config/gazebo_ros_control_params.yaml" command="load"/>
<!-- Call Nao Robot Trajectory Controller -->
<include file="$(find nao_dcm_control)/launch/nao_dcm_trajectory_control_H25.launch"/>
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -x 0 -y 0 -z 0.339 -model nao_robot_v4_H25" />
</launch>

39
nao_dcm_apps/nao_dcm_gazebo/package.xml Normal file
View File

@ -0,0 +1,39 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm_gazebo</name>
<version>0.0.1</version>
<description>Simulate Aldebaran's NAO robot (v4) in Gazebo simulator</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>gazebo_ros</build_depend>
<run_depend>nao_dcm_description</run_depend>
<run_depend>gazebo_ros</run_depend>
<export>
</export>
</package>

16
nao_dcm_apps/nao_dcm_gazebo/worlds/nao_test.world Normal file
View File

@ -0,0 +1,16 @@
<?xml version="1.0" ?>
<sdf version="1.4">
<world name="default">
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<include>
<uri>model://gas_station</uri>
<name>gas_station</name>
<pose>-2.0 7.0 0 0 0 0</pose>
</include>
</world>
</sdf>

8
nao_dcm_apps/nao_dcm_moveit_config/.setup_assistant Normal file
View File

@ -0,0 +1,8 @@
moveit_setup_assistant_config:
URDF:
package: nao_dcm_description
relative_path: urdf/nao_dcm_H25.xacro
SRDF:
relative_path: config/nao_robot_v4_H25.srdf
CONFIG:
generated_timestamp: 1398368537

12
nao_dcm_apps/nao_dcm_moveit_config/CMakeLists.txt Normal file
View File

@ -0,0 +1,12 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm_moveit_config)
find_package(catkin REQUIRED)
catkin_package()
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

53
nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H21.yaml Normal file
View File

@ -0,0 +1,53 @@
controller_manager_ns: ''
controller_list:
- name: nao_dcm/LeftArm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LShoulderPitch
- LShoulderRoll
- LElbowYaw
- LElbowRoll
- name: nao_dcm/RightArm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- RShoulderPitch
- RShoulderRoll
- RElbowYaw
- RElbowRoll
- name: nao_dcm/Pelvis_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LHipYawPitch
- name: nao_dcm/LeftLeg_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LHipRoll
- LHipPitch
- LKneePitch
- LAnklePitch
- LAnkleRoll
- name: nao_dcm/RightLeg_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- RHipRoll
- RHipPitch
- RKneePitch
- RAnklePitch
- RAnkleRoll
- name: nao_dcm/Head_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- HeadYaw
- HeadPitch

67
nao_dcm_apps/nao_dcm_moveit_config/config/controllers_H25.yaml Normal file
View File

@ -0,0 +1,67 @@
controller_manager_ns: ''
controller_list:
- name: nao_dcm/LeftArm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LShoulderPitch
- LShoulderRoll
- LElbowYaw
- LElbowRoll
- name: nao_dcm/RightArm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- RShoulderPitch
- RShoulderRoll
- RElbowYaw
- RElbowRoll
- name: nao_dcm/LeftHand_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LWristYaw
- LHand
- name: nao_dcm/RightHand_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- RWristYaw
- RHand
- name: nao_dcm/Pelvis_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LHipYawPitch
- name: nao_dcm/LeftLeg_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- LHipRoll
- LHipPitch
- LKneePitch
- LAnklePitch
- LAnkleRoll
- name: nao_dcm/RightLeg_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- RHipRoll
- RHipPitch
- RKneePitch
- RAnklePitch
- RAnkleRoll
- name: nao_dcm/Head_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- HeadYaw
- HeadPitch

42
nao_dcm_apps/nao_dcm_moveit_config/config/fake_controllers.yaml Normal file
View File

@ -0,0 +1,42 @@
controller_list:
- name: fake_LeftArm_controller
joints:
- LShoulderPitch
- LShoulderRoll
- LElbowYaw
- LElbowRoll
- name: fake_RightArm_controller
joints:
- RShoulderPitch
- RShoulderRoll
- RElbowYaw
- RElbowRoll
- name: fake_Head_controller
joints:
- HeadYaw
- HeadPitch
- name: fake_LeftLeg_controller
joints:
- LHipRoll
- LHipPitch
- LKneePitch
- LAnklePitch
- LAnkleRoll
- name: fake_RightLeg_controller
joints:
- RHipRoll
- RHipPitch
- RKneePitch
- RAnklePitch
- RAnkleRoll
- name: fake_Pelvis_controller
joints:
- LHipYawPitch
- name: fake_RightHand_controller
joints:
- RWristYaw
- RHand
- name: fake_LeftHand_controller
joints:
- LWristYaw
- LHand

126
nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits.yaml Normal file
View File

@ -0,0 +1,126 @@
joint_limits:
HeadPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
HeadYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LAnklePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LAnkleRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LElbowRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LElbowYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHand:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHipPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHipRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHipYawPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LKneePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LShoulderPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LShoulderRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LWristYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RAnklePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RAnkleRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RElbowRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RElbowYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RHand:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RHipPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RHipRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RKneePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RShoulderPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RShoulderRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RWristYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0

106
nao_dcm_apps/nao_dcm_moveit_config/config/joint_limits_H21.yaml Normal file
View File

@ -0,0 +1,106 @@
joint_limits:
HeadPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
HeadYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LAnklePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LAnkleRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LElbowRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LElbowYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHipPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHipRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LHipYawPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LKneePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LShoulderPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
LShoulderRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RAnklePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RAnkleRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RElbowRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RElbowYaw:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RHipPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RHipRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RKneePitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RShoulderPitch:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0
RShoulderRoll:
has_velocity_limits: true
max_velocity: 3
has_acceleration_limits: false
max_acceleration: 0

40
nao_dcm_apps/nao_dcm_moveit_config/config/kinematics.yaml Normal file
View File

@ -0,0 +1,40 @@
LeftArm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
RightArm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
Head:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
LeftLeg:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
RightLeg:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
Pelvis:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
RightHand:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
LeftHand:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3

30
nao_dcm_apps/nao_dcm_moveit_config/config/kinematics_H21.yaml Normal file
View File

@ -0,0 +1,30 @@
LeftArm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
RightArm:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
Head:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
LeftLeg:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
RightLeg:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3
Pelvis:
kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
kinematics_solver_search_resolution: 0.005
kinematics_solver_timeout: 0.005
kinematics_solver_attempts: 3

465
nao_dcm_apps/nao_dcm_moveit_config/config/nao_dcm_moveit_config.rviz Normal file
View File

@ -0,0 +1,465 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /MotionPlanning1
- /MotionPlanning1/Planning Request1
Splitter Ratio: 0.5
Tree Height: 330
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
MoveIt_Goal_Tolerance: 0
MoveIt_Planning_Time: 5
MoveIt_Use_Constraint_Aware_IK: true
MoveIt_Warehouse_Host: 127.0.0.1
MoveIt_Warehouse_Port: 33829
Name: MotionPlanning
Planned Path:
Links:
All Links Enabled: true
CameraBottom:
Alpha: 1
Show Axes: false
Show Trail: false
CameraTop:
Alpha: 1
Show Axes: false
Show Trail: false
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
LElbow:
Alpha: 1
Show Axes: false
Show Trail: false
LElbowDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LHip:
Alpha: 1
Show Axes: false
Show Trail: false
LHipDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LHipYaw:
Alpha: 1
Show Axes: false
Show Trail: false
LKnee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LShoulder:
Alpha: 1
Show Axes: false
Show Trail: false
LShoulderDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RElbow:
Alpha: 1
Show Axes: false
Show Trail: false
RElbowDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RHip:
Alpha: 1
Show Axes: false
Show Trail: false
RHipDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RHipYaw:
Alpha: 1
Show Axes: false
Show Trail: false
RKnee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RShoulder:
Alpha: 1
Show Axes: false
Show Trail: false
RShoulderDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
SonarLeft:
Alpha: 1
Show Axes: false
Show Trail: false
SonarRight:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
gaze:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_ankle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper:
Alpha: 1
Show Axes: false
Show Trail: false
l_sole:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
neck:
Alpha: 1
Show Axes: false
Show Trail: false
r_ankle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper:
Alpha: 1
Show Axes: false
Show Trail: false
r_sole:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: false
Robot Alpha: 0.5
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
Trajectory Topic: /move_group/display_planned_path
Planning Metrics:
Payload: 1
Show Frobenius Condition Number: false
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Minimum Singular Value: false
Show Weight Limit: false
TextHeight: 0.08
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
Planning Group: LeftArm
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 1
Start State Color: 0; 255; 0
Planning Scene Topic: /move_group/monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 0.9
Scene Color: 50; 230; 50
Scene Display Time: 0.2
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
All Links Enabled: true
CameraBottom:
Alpha: 1
Show Axes: false
Show Trail: false
CameraTop:
Alpha: 1
Show Axes: false
Show Trail: false
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
LElbow:
Alpha: 1
Show Axes: false
Show Trail: false
LElbowDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LHip:
Alpha: 1
Show Axes: false
Show Trail: false
LHipDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LHipYaw:
Alpha: 1
Show Axes: false
Show Trail: false
LKnee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LShoulder:
Alpha: 1
Show Axes: false
Show Trail: false
LShoulderDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RElbow:
Alpha: 1
Show Axes: false
Show Trail: false
RElbowDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RHip:
Alpha: 1
Show Axes: false
Show Trail: false
RHipDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RHipYaw:
Alpha: 1
Show Axes: false
Show Trail: false
RKnee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RShoulder:
Alpha: 1
Show Axes: false
Show Trail: false
RShoulderDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
SonarLeft:
Alpha: 1
Show Axes: false
Show Trail: false
SonarRight:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
gaze:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_ankle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper:
Alpha: 1
Show Axes: false
Show Trail: false
l_sole:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
neck:
Alpha: 1
Show Axes: false
Show Trail: false
r_ankle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper:
Alpha: 1
Show Axes: false
Show Trail: false
r_sole:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 0.5
Show Robot Collision: false
Show Robot Visual: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.79867
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.440398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.240398
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1031
Hide Left Dock: false
Hide Right Dock: false
Motion Planning:
collapsed: false
QMainWindow State: 000000ff00000000fd00000004000000000000027c00000362fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000043000001dc000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000225000001800000017500ffffff000000010000010f00000362fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000362000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000023a00fffffffb0000000800540069006d00650100000000000004500000000000000000000003e90000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 0
Y: 25

130
nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H21.srdf Normal file
View File

@ -0,0 +1,130 @@
<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="nao_robot_v4_H21">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="LeftArm">
<joint name="LShoulderPitch" />
<joint name="LShoulderRoll" />
<joint name="LElbowYaw" />
<joint name="LElbowRoll" />
</group>
<group name="RightArm">
<joint name="RShoulderPitch" />
<joint name="RShoulderRoll" />
<joint name="RElbowYaw" />
<joint name="RElbowRoll" />
</group>
<group name="Head">
<joint name="HeadYaw" />
<joint name="HeadPitch" />
</group>
<group name="LeftLeg">
<joint name="LHipRoll" />
<joint name="LHipPitch" />
<joint name="LKneePitch" />
<joint name="LAnklePitch" />
<joint name="LAnkleRoll" />
</group>
<group name="RightLeg">
<joint name="RHipRoll" />
<joint name="RHipPitch" />
<joint name="RKneePitch" />
<joint name="RAnklePitch" />
<joint name="RAnkleRoll" />
</group>
<group name="Pelvis">
<joint name="LHipYawPitch" />
</group>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="floating" parent_frame="odom_combined" child_link="base_link" />
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<passive_joint name="virtual_joint" />
<passive_joint name="RHipYawPitch" />
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="LElbowDummy" link2="LShoulderDummy" reason="Adjacent" />
<disable_collisions link1="LElbowDummy" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="RHipDummy" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="RKnee" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="l_wrist" reason="Adjacent" />
<disable_collisions link1="LElbowDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="r_sole" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="LHipDummy" link2="LKnee" reason="Adjacent" />
<disable_collisions link1="LHipDummy" link2="LShoulderDummy" reason="Never" />
<disable_collisions link1="LHipDummy" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LHipDummy" link2="RHipDummy" reason="Adjacent" />
<disable_collisions link1="LHipDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LHipDummy" link2="gaze" reason="Never" />
<disable_collisions link1="LHipDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="LHipDummy" link2="l_sole" reason="Never" />
<disable_collisions link1="LHipDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="LHipDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="LHipDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="LKnee" link2="LShoulderDummy" reason="Never" />
<disable_collisions link1="LKnee" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LKnee" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LKnee" link2="gaze" reason="Never" />
<disable_collisions link1="LKnee" link2="l_ankle" reason="Adjacent" />
<disable_collisions link1="LKnee" link2="l_sole" reason="Never" />
<disable_collisions link1="LKnee" link2="r_wrist" reason="Never" />
<disable_collisions link1="LKnee" link2="torso" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RHipDummy" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RKnee" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="r_sole" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="RElbowDummy" link2="RShoulderDummy" reason="Adjacent" />
<disable_collisions link1="RElbowDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="l_sole" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="r_wrist" reason="Adjacent" />
<disable_collisions link1="RHipDummy" link2="RKnee" reason="Adjacent" />
<disable_collisions link1="RHipDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="RHipDummy" link2="gaze" reason="Never" />
<disable_collisions link1="RHipDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="RHipDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="RHipDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="RHipDummy" link2="r_sole" reason="Never" />
<disable_collisions link1="RHipDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="RKnee" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="RKnee" link2="gaze" reason="Never" />
<disable_collisions link1="RKnee" link2="l_wrist" reason="Never" />
<disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
<disable_collisions link1="RKnee" link2="r_sole" reason="Never" />
<disable_collisions link1="RKnee" link2="torso" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="l_sole" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="gaze" link2="l_ankle" reason="Never" />
<disable_collisions link1="gaze" link2="l_sole" reason="Never" />
<disable_collisions link1="gaze" link2="r_ankle" reason="Never" />
<disable_collisions link1="gaze" link2="r_sole" reason="Never" />
<disable_collisions link1="gaze" link2="torso" reason="Adjacent" />
<disable_collisions link1="l_ankle" link2="l_sole" reason="Adjacent" />
<disable_collisions link1="l_ankle" link2="l_wrist" reason="Never" />
<disable_collisions link1="l_ankle" link2="r_wrist" reason="Never" />
<disable_collisions link1="l_ankle" link2="torso" reason="Never" />
<disable_collisions link1="l_sole" link2="r_wrist" reason="Never" />
<disable_collisions link1="l_wrist" link2="r_ankle" reason="Never" />
<disable_collisions link1="l_wrist" link2="r_sole" reason="Never" />
<disable_collisions link1="r_ankle" link2="r_sole" reason="Adjacent" />
<disable_collisions link1="r_ankle" link2="torso" reason="Never" />
</robot>

157
nao_dcm_apps/nao_dcm_moveit_config/config/nao_robot_v4_H25.srdf Normal file
View File

@ -0,0 +1,157 @@
<?xml version="1.0" ?>
<!--This does not replace URDF, and is not an extension of URDF.
This is a format for representing semantic information about the robot structure.
A URDF file must exist for this robot as well, where the joints and the links that are referenced are defined
-->
<robot name="nao_robot_v4_H25">
<!--GROUPS: Representation of a set of joints and links. This can be useful for specifying DOF to plan for, defining arms, end effectors, etc-->
<!--LINKS: When a link is specified, the parent joint of that link (if it exists) is automatically included-->
<!--JOINTS: When a joint is specified, the child link of that joint (which will always exist) is automatically included-->
<!--CHAINS: When a chain is specified, all the links along the chain (including endpoints) are included in the group. Additionally, all the joints that are parents to included links are also included. This means that joints along the chain and the parent joint of the base link are included in the group-->
<!--SUBGROUPS: Groups can also be formed by referencing to already defined group names-->
<group name="LeftArm">
<joint name="LShoulderPitch" />
<joint name="LShoulderRoll" />
<joint name="LElbowYaw" />
<joint name="LElbowRoll" />
</group>
<group name="RightArm">
<joint name="RShoulderPitch" />
<joint name="RShoulderRoll" />
<joint name="RElbowYaw" />
<joint name="RElbowRoll" />
</group>
<group name="Head">
<joint name="HeadYaw" />
<joint name="HeadPitch" />
</group>
<group name="LeftLeg">
<joint name="LHipRoll" />
<joint name="LHipPitch" />
<joint name="LKneePitch" />
<joint name="LAnklePitch" />
<joint name="LAnkleRoll" />
</group>
<group name="RightLeg">
<joint name="RHipRoll" />
<joint name="RHipPitch" />
<joint name="RKneePitch" />
<joint name="RAnklePitch" />
<joint name="RAnkleRoll" />
</group>
<group name="Pelvis">
<joint name="LHipYawPitch" />
</group>
<group name="RightHand">
<joint name="RWristYaw" />
<joint name="RHand" />
</group>
<group name="LeftHand">
<joint name="LHand" />
<joint name="LWristYaw" />
</group>
<group name="RightArmHand">
<joint name="RShoulderPitch" />
<joint name="RShoulderRoll" />
<joint name="RElbowYaw" />
<joint name="RElbowRoll" />
<joint name="RWristYaw" />
<joint name="RHand" />
</group>
<group name="LeftArmHand">
<joint name="LShoulderPitch" />
<joint name="LShoulderRoll" />
<joint name="LElbowYaw" />
<joint name="LElbowRoll" />
<joint name="LWristYaw" />
<joint name="LHand" />
</group>
<!--END EFFECTOR: Purpose: Represent information about an end effector.-->
<end_effector name="right_eef" parent_link="RElbowDummy" group="RightHand" parent_group="RightArm" />
<end_effector name="left_eef" parent_link="LElbowDummy" group="LeftHand" parent_group="LeftArm" />
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
<virtual_joint name="virtual_joint" type="floating" parent_frame="odom_combined" child_link="base_link" />
<!--PASSIVE JOINT: Purpose: this element is used to mark joints that are not actuated-->
<passive_joint name="virtual_joint" />
<passive_joint name="RHipYawPitch" />
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="LElbowDummy" link2="LShoulderDummy" reason="Adjacent" />
<disable_collisions link1="LElbowDummy" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="RHipDummy" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="RKnee" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="l_wrist" reason="Adjacent" />
<disable_collisions link1="LElbowDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="r_sole" reason="Never" />
<disable_collisions link1="LElbowDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="LHipDummy" link2="LKnee" reason="Adjacent" />
<disable_collisions link1="LHipDummy" link2="LShoulderDummy" reason="Never" />
<disable_collisions link1="LHipDummy" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LHipDummy" link2="RHipDummy" reason="Adjacent" />
<disable_collisions link1="LHipDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LHipDummy" link2="gaze" reason="Never" />
<disable_collisions link1="LHipDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="LHipDummy" link2="l_sole" reason="Never" />
<disable_collisions link1="LHipDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="LHipDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="LHipDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="LKnee" link2="LShoulderDummy" reason="Never" />
<disable_collisions link1="LKnee" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LKnee" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LKnee" link2="gaze" reason="Never" />
<disable_collisions link1="LKnee" link2="l_ankle" reason="Adjacent" />
<disable_collisions link1="LKnee" link2="l_sole" reason="Never" />
<disable_collisions link1="LKnee" link2="r_wrist" reason="Never" />
<disable_collisions link1="LKnee" link2="torso" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RElbowDummy" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RHipDummy" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RKnee" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="r_sole" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="LShoulderDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="RElbowDummy" link2="RShoulderDummy" reason="Adjacent" />
<disable_collisions link1="RElbowDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="l_sole" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="RElbowDummy" link2="r_wrist" reason="Adjacent" />
<disable_collisions link1="RHipDummy" link2="RKnee" reason="Adjacent" />
<disable_collisions link1="RHipDummy" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="RHipDummy" link2="gaze" reason="Never" />
<disable_collisions link1="RHipDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="RHipDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="RHipDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="RHipDummy" link2="r_sole" reason="Never" />
<disable_collisions link1="RHipDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="RKnee" link2="RShoulderDummy" reason="Never" />
<disable_collisions link1="RKnee" link2="gaze" reason="Never" />
<disable_collisions link1="RKnee" link2="l_wrist" reason="Never" />
<disable_collisions link1="RKnee" link2="r_ankle" reason="Adjacent" />
<disable_collisions link1="RKnee" link2="r_sole" reason="Never" />
<disable_collisions link1="RKnee" link2="torso" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="l_ankle" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="l_sole" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="l_wrist" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="r_ankle" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="r_wrist" reason="Never" />
<disable_collisions link1="RShoulderDummy" link2="torso" reason="Adjacent" />
<disable_collisions link1="gaze" link2="l_ankle" reason="Never" />
<disable_collisions link1="gaze" link2="l_sole" reason="Never" />
<disable_collisions link1="gaze" link2="r_ankle" reason="Never" />
<disable_collisions link1="gaze" link2="r_sole" reason="Never" />
<disable_collisions link1="gaze" link2="torso" reason="Adjacent" />
<disable_collisions link1="l_ankle" link2="l_sole" reason="Adjacent" />
<disable_collisions link1="l_ankle" link2="l_wrist" reason="Never" />
<disable_collisions link1="l_ankle" link2="r_wrist" reason="Never" />
<disable_collisions link1="l_ankle" link2="torso" reason="Never" />
<disable_collisions link1="l_sole" link2="r_wrist" reason="Never" />
<disable_collisions link1="l_wrist" link2="r_ankle" reason="Never" />
<disable_collisions link1="l_wrist" link2="r_sole" reason="Never" />
<disable_collisions link1="r_ankle" link2="r_sole" reason="Adjacent" />
<disable_collisions link1="r_ankle" link2="torso" reason="Never" />
</robot>

111
nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H21.yaml Normal file
View File

@ -0,0 +1,111 @@
planner_configs:
SBLkConfigDefault:
type: geometric::SBL
ESTkConfigDefault:
type: geometric::EST
LBKPIECEkConfigDefault:
type: geometric::LBKPIECE
BKPIECEkConfigDefault:
type: geometric::BKPIECE
KPIECEkConfigDefault:
type: geometric::KPIECE
RRTkConfigDefault:
type: geometric::RRT
RRTConnectkConfigDefault:
type: geometric::RRTConnect
RRTstarkConfigDefault:
type: geometric::RRTstar
TRRTkConfigDefault:
type: geometric::TRRT
PRMkConfigDefault:
type: geometric::PRM
PRMstarkConfigDefault:
type: geometric::PRMstar
LeftArm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(LShoulderPitch,LShoulderRoll)
longest_valid_segment_fraction: 0.05
RightArm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(RShoulderPitch,RShoulderRoll)
longest_valid_segment_fraction: 0.05
Head:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(HeadYaw,HeadPitch)
longest_valid_segment_fraction: 0.05
LeftLeg:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(LHipRoll,LHipPitch)
longest_valid_segment_fraction: 0.05
RightLeg:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(RHipRoll,RHipPitch)
longest_valid_segment_fraction: 0.05
Pelvis:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault

141
nao_dcm_apps/nao_dcm_moveit_config/config/ompl_planning_H25.yaml Normal file
View File

@ -0,0 +1,141 @@
planner_configs:
SBLkConfigDefault:
type: geometric::SBL
ESTkConfigDefault:
type: geometric::EST
LBKPIECEkConfigDefault:
type: geometric::LBKPIECE
BKPIECEkConfigDefault:
type: geometric::BKPIECE
KPIECEkConfigDefault:
type: geometric::KPIECE
RRTkConfigDefault:
type: geometric::RRT
RRTConnectkConfigDefault:
type: geometric::RRTConnect
RRTstarkConfigDefault:
type: geometric::RRTstar
TRRTkConfigDefault:
type: geometric::TRRT
PRMkConfigDefault:
type: geometric::PRM
PRMstarkConfigDefault:
type: geometric::PRMstar
LeftArm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(LShoulderPitch,LShoulderRoll)
longest_valid_segment_fraction: 0.05
RightArm:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(RShoulderPitch,RShoulderRoll)
longest_valid_segment_fraction: 0.05
Head:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(HeadYaw,HeadPitch)
longest_valid_segment_fraction: 0.05
LeftLeg:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(LHipRoll,LHipPitch)
longest_valid_segment_fraction: 0.05
RightLeg:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(RHipRoll,RHipPitch)
longest_valid_segment_fraction: 0.05
Pelvis:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
RightHand:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(RWristYaw,RHand)
longest_valid_segment_fraction: 0.05
LeftHand:
planner_configs:
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
- BKPIECEkConfigDefault
- KPIECEkConfigDefault
- RRTkConfigDefault
- RRTConnectkConfigDefault
- RRTstarkConfigDefault
- TRRTkConfigDefault
- PRMkConfigDefault
- PRMstarkConfigDefault
projection_evaluator: joints(LWristYaw,LHand)
longest_valid_segment_fraction: 0.05

13
nao_dcm_apps/nao_dcm_moveit_config/launch/default_warehouse_db.launch Normal file
View File

@ -0,0 +1,13 @@
<launch>
<arg name="reset" default="false"/>
<!-- Launch the warehouse with a default database location -->
<include file="$(find nao_dcm_moveit_config)/launch/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="$(find nao_dcm_moveit_config)/default_warehouse_mongo_db" />
</include>
<!-- If we want to reset the database, run this node -->
<node if="$(arg reset)" name="$(anon moveit_default_db_reset)" type="moveit_init_demo_warehouse" pkg="moveit_ros_warehouse" respawn="false" output="screen" />
</launch>

44
nao_dcm_apps/nao_dcm_moveit_config/launch/demo.launch Normal file
View File

@ -0,0 +1,44 @@
<launch>
<!-- By default, we do not start a database (it can be large) -->
<arg name="db" default="false" />
<!-- By default, we are not in debug mode -->
<arg name="debug" default="false" />
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
<include file="$(find nao_dcm_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- If needed, broadcast static tf for robot root -->
<node pkg="tf" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 odom base_link 100" />
<!-- We do not have a robot connected, so publish fake joint states -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
<param name="/use_gui" value="false"/>
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
</node>
<!-- Given the published joint states, publish tf for the robot links -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
<include file="$(find nao_dcm_moveit_config)/launch/move_group.launch">
<arg name="allow_trajectory_execution" value="true"/>
<arg name="fake_execution" value="true"/>
<arg name="info" value="true"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- Run Rviz and load the default config to see the state of the move_group node -->
<include file="$(find nao_dcm_moveit_config)/launch/moveit_rviz.launch">
<arg name="config" value="true"/>
<arg name="debug" value="$(arg debug)"/>
</include>
<!-- If database loading was enabled, start mongodb as well -->
<include file="$(find nao_dcm_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"/>
</launch>

9
nao_dcm_apps/nao_dcm_moveit_config/launch/fake_moveit_controller_manager.launch.xml Normal file
View File

@ -0,0 +1,9 @@
<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<param name="moveit_controller_manager" value="moveit_fake_controller_manager/MoveItFakeControllerManager"/>
<!-- The rest of the params are specific to this plugin -->
<rosparam file="$(find nao_dcm_moveit_config)/config/fake_controllers.yaml"/>
</launch>

70
nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H21.launch Normal file
View File

@ -0,0 +1,70 @@
<launch>
<include file="$(find nao_dcm_moveit_config)/launch/planning_context_H21.launch" />
<!-- GDB Debug Option -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<!-- Verbose Mode Option -->
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />
<!-- move_group settings -->
<arg name="allow_trajectory_execution" default="true"/>
<arg name="fake_execution" default="false"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>
<!-- Planning Functionality -->
<include ns="move_group" file="$(find nao_dcm_moveit_config)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
<arg name="body_type" value="H21" />
</include>
<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(find nao_dcm_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="nao_robot_v4_H21" unless="$(arg fake_execution)"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
</include>
<!-- Sensors Functionality -->
<include ns="move_group" file="$(find nao_dcm_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_sensor_manager" value="nao_robot_v4_H21" />
</include>
<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<!-- Set the display variable, in case OpenGL code is used internally -->
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<!-- MoveGroup capabilities to load -->
<param name="capabilities" value="move_group/MoveGroupCartesianPathService
move_group/MoveGroupExecuteService
move_group/MoveGroupKinematicsService
move_group/MoveGroupMoveAction
move_group/MoveGroupPickPlaceAction
move_group/MoveGroupPlanService
move_group/MoveGroupQueryPlannersService
move_group/MoveGroupStateValidationService
move_group/MoveGroupGetPlanningSceneService
" />
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
<!-- Remap joint_states to correct namespace -->
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
</node>
</launch>

70
nao_dcm_apps/nao_dcm_moveit_config/launch/move_group_H25.launch Normal file
View File

@ -0,0 +1,70 @@
<launch>
<include file="$(find nao_dcm_moveit_config)/launch/planning_context_H25.launch" />
<!-- GDB Debug Option -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<!-- Verbose Mode Option -->
<arg name="info" default="$(arg debug)" />
<arg unless="$(arg info)" name="command_args" value="" />
<arg if="$(arg info)" name="command_args" value="--debug" />
<!-- move_group settings -->
<arg name="allow_trajectory_execution" default="true"/>
<arg name="fake_execution" default="false"/>
<arg name="max_safe_path_cost" default="1"/>
<arg name="jiggle_fraction" default="0.05" />
<arg name="publish_monitored_planning_scene" default="true"/>
<!-- Planning Functionality -->
<include ns="move_group" file="$(find nao_dcm_moveit_config)/launch/planning_pipeline.launch.xml">
<arg name="pipeline" value="ompl" />
<arg name="body_type" value="H25" />
</include>
<!-- Trajectory Execution Functionality -->
<include ns="move_group" file="$(find nao_dcm_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_manage_controllers" value="true" />
<arg name="moveit_controller_manager" value="nao_robot_v4_H25" unless="$(arg fake_execution)"/>
<arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
</include>
<!-- Sensors Functionality -->
<include ns="move_group" file="$(find nao_dcm_moveit_config)/launch/sensor_manager.launch.xml" if="$(arg allow_trajectory_execution)">
<arg name="moveit_sensor_manager" value="nao_robot_v4_H25" />
</include>
<!-- Start the actual move_group node/action server -->
<node name="move_group" launch-prefix="$(arg launch_prefix)" pkg="moveit_ros_move_group" type="move_group" respawn="false" output="screen" args="$(arg command_args)">
<!-- Set the display variable, in case OpenGL code is used internally -->
<env name="DISPLAY" value="$(optenv DISPLAY :0)" />
<param name="allow_trajectory_execution" value="$(arg allow_trajectory_execution)"/>
<param name="max_safe_path_cost" value="$(arg max_safe_path_cost)"/>
<param name="jiggle_fraction" value="$(arg jiggle_fraction)" />
<!-- MoveGroup capabilities to load -->
<param name="capabilities" value="move_group/MoveGroupCartesianPathService
move_group/MoveGroupExecuteService
move_group/MoveGroupKinematicsService
move_group/MoveGroupMoveAction
move_group/MoveGroupPickPlaceAction
move_group/MoveGroupPlanService
move_group/MoveGroupQueryPlannersService
move_group/MoveGroupStateValidationService
move_group/MoveGroupGetPlanningSceneService
" />
<!-- Publish the planning scene of the physical robot so that rviz plugin can know actual robot -->
<param name="planning_scene_monitor/publish_planning_scene" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_geometry_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_state_updates" value="$(arg publish_monitored_planning_scene)" />
<param name="planning_scene_monitor/publish_transforms_updates" value="$(arg publish_monitored_planning_scene)" />
<!-- Remap joint_states to correct namespace -->
<remap from="/joint_states" to="/nao_dcm/joint_states"/>
</node>
</launch>

689
nao_dcm_apps/nao_dcm_moveit_config/launch/moveit.rviz Normal file
View File

@ -0,0 +1,689 @@
Panels:
- Class: rviz/Displays
Help Height: 84
Name: Displays
Property Tree Widget:
Expanded:
- /MotionPlanning1
Splitter Ratio: 0.74256
Tree Height: 664
- Class: rviz/Help
Name: Help
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: moveit_rviz_plugin/MotionPlanning
Enabled: true
MoveIt_Goal_Tolerance: 0
MoveIt_Planning_Time: 5
MoveIt_Use_Constraint_Aware_IK: true
MoveIt_Warehouse_Host: 127.0.0.1
MoveIt_Warehouse_Port: 33829
Name: MotionPlanning
Planned Path:
Links:
base_bellow_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
double_stereo_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_rgb_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_prosilica_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_plate_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_tilt_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
laser_tilt_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
sensor_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Loop Animation: true
Robot Alpha: 0.5
Show Robot Collision: false
Show Robot Visual: true
Show Trail: false
State Display Time: 0.05 s
Trajectory Topic: /move_group/display_planned_path
Planning Metrics:
Payload: 1
Show Joint Torques: false
Show Manipulability: false
Show Manipulability Index: false
Show Weight Limit: false
Planning Request:
Colliding Link Color: 255; 0; 0
Goal State Alpha: 1
Goal State Color: 250; 128; 0
Interactive Marker Size: 0
Joint Violation Color: 255; 0; 255
Planning Group: left_arm
Query Goal State: true
Query Start State: false
Show Workspace: false
Start State Alpha: 1
Start State Color: 0; 255; 0
Planning Scene Topic: /move_group/monitored_planning_scene
Robot Description: robot_description
Scene Geometry:
Scene Alpha: 1
Scene Color: 50; 230; 50
Scene Display Time: 0.2
Show Scene Geometry: true
Voxel Coloring: Z-Axis
Voxel Rendering: Occupied Voxels
Scene Robot:
Attached Body Color: 150; 50; 150
Links:
base_bellow_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_footprint:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
bl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
br_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
double_stereo_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fl_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_l_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_r_wheel_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
fr_caster_rotation_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_ir_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_kinect_rgb_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_mount_prosilica_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_plate_frame:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
head_tilt_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
laser_tilt_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_elbow_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_forearm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_l_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_motor_accelerometer_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_palm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_gripper_r_finger_tip_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_shoulder_pan_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_upper_arm_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_flex_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist_roll_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
sensor_mount_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso_lift_link:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Robot Alpha: 0.5
Show Scene Robot: true
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: /odom
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
Value: true
Views:
Current:
Class: rviz/XYOrbit
Distance: 2.9965
Focal Point:
X: 0.113567
Y: 0.10592
Z: 2.23518e-07
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.509797
Target Frame: /base_link
Value: XYOrbit (rviz)
Yaw: 5.65995
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1337
Help:
collapsed: false
Hide Left Dock: false
Hide Right Dock: false
Motion Planning:
collapsed: false
QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Views:
collapsed: false
Width: 1828
X: 459
Y: -243

11
nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H21.launch Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<!-- Start move_group -->
<include file="$(find nao_dcm_moveit_config)/launch/move_group_H21.launch">
<arg name="publish_monitored_planning_scene" value="true" />
</include>
<!-- Start Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nao_dcm_moveit_config)/config/nao_dcm_moveit_config.rviz" />
</launch>

11
nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_planner_H25.launch Normal file
View File

@ -0,0 +1,11 @@
<?xml version="1.0"?>
<launch>
<!-- Start move_group -->
<include file="$(find nao_dcm_moveit_config)/launch/move_group_H25.launch">
<arg name="publish_monitored_planning_scene" value="true" />
</include>
<!-- Start Rviz -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nao_dcm_moveit_config)/config/nao_dcm_moveit_config.rviz" />
</launch>

16
nao_dcm_apps/nao_dcm_moveit_config/launch/moveit_rviz.launch Normal file
View File

@ -0,0 +1,16 @@
<launch>
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="config" default="false" />
<arg unless="$(arg config)" name="command_args" value="" />
<arg if="$(arg config)" name="command_args" value="-d $(find nao_dcm_moveit_config)/launch/moveit.rviz" />
<node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
args="$(arg command_args)" output="screen">
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/kinematics.yaml"/>
</node>
</launch>

12
nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_controller_manager.launch.xml Normal file
View File

@ -0,0 +1,12 @@
<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- load controller_list -->
<arg name="use_controller_manager" default="true" />
<param name="use_controller_manager" value="$(arg use_controller_manager)" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_moveit_config)/config/controllers_H21.yaml"/>
</launch>

3
nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H21_moveit_sensor_manager.launch.xml Normal file
View File

@ -0,0 +1,3 @@
<launch>
</launch>

12
nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_controller_manager.launch.xml Normal file
View File

@ -0,0 +1,12 @@
<launch>
<!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
<!-- load controller_list -->
<arg name="use_controller_manager" default="true" />
<param name="use_controller_manager" value="$(arg use_controller_manager)" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_moveit_config)/config/controllers_H25.yaml"/>
</launch>

3
nao_dcm_apps/nao_dcm_moveit_config/launch/nao_robot_v4_H25_moveit_sensor_manager.launch.xml Normal file
View File

@ -0,0 +1,3 @@
<launch>
</launch>

23
nao_dcm_apps/nao_dcm_moveit_config/launch/ompl_planning_pipeline.launch.xml Normal file
View File

@ -0,0 +1,23 @@
<launch>
<!-- OMPL Plugin for MoveIt! -->
<arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />
<arg name="body" default="H25" />
<!-- The request adapters (plugins) used when planning with OMPL.
ORDER MATTERS -->
<arg name="planning_adapters" value="default_planner_request_adapters/AddTimeParameterization
default_planner_request_adapters/FixWorkspaceBounds
default_planner_request_adapters/FixStartStateBounds
default_planner_request_adapters/FixStartStateCollision
default_planner_request_adapters/FixStartStatePathConstraints" />
<arg name="start_state_max_bounds_error" value="0.1" />
<param name="planning_plugin" value="$(arg planning_plugin)" />
<param name="request_adapters" value="$(arg planning_adapters)" />
<param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/ompl_planning_$(arg body).yaml"/>
</launch>

24
nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H21.launch Normal file
View File

@ -0,0 +1,24 @@
<launch>
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_robot_description" default="false"/>
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
<param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro.py '$(find nao_dcm_description)/urdf/nao_dcm_H21.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find nao_dcm_moveit_config)/config/nao_robot_v4_H21.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/joint_limits_H21.yaml"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/kinematics_H21.yaml"/>
</group>
</launch>

24
nao_dcm_apps/nao_dcm_moveit_config/launch/planning_context_H25.launch Normal file
View File

@ -0,0 +1,24 @@
<launch>
<!-- By default we do not overwrite the URDF. Change the following to true to change the default behavior -->
<arg name="load_robot_description" default="false"/>
<!-- The name of the parameter under which the URDF is loaded -->
<arg name="robot_description" default="robot_description"/>
<!-- Load universal robot description format (URDF) -->
<param if="$(arg load_robot_description)" name="$(arg robot_description)" command="$(find xacro)/xacro.py '$(find nao_dcm_description)/urdf/nao_dcm_H25.xacro'"/>
<!-- The semantic description that corresponds to the URDF -->
<param name="$(arg robot_description)_semantic" textfile="$(find nao_dcm_moveit_config)/config/nao_robot_v4_H25.srdf" />
<!-- Load updated joint limits (override information from URDF) -->
<group ns="$(arg robot_description)_planning">
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/joint_limits.yaml"/>
</group>
<!-- Load default settings for kinematics; these settings are overridden by settings in a node's namespace -->
<group ns="$(arg robot_description)_kinematics">
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/kinematics.yaml"/>
</group>
</launch>

13
nao_dcm_apps/nao_dcm_moveit_config/launch/planning_pipeline.launch.xml Normal file
View File

@ -0,0 +1,13 @@
<launch>
<!-- This file makes it easy to include different planning pipelines;
It is assumed that all planning pipelines are named XXX_planning_pipeline.launch -->
<arg name="pipeline" default="ompl" />
<arg name="body_type" default="H25" />
<include file="$(find nao_dcm_moveit_config)/launch/$(arg pipeline)_planning_pipeline.launch.xml" >
<arg name="body" value="$(arg body_type)" />
</include>
</launch>

22
nao_dcm_apps/nao_dcm_moveit_config/launch/run_benchmark_ompl.launch Normal file
View File

@ -0,0 +1,22 @@
<launch>
<!-- This argument must specify the list of .cfg files to process for benchmarking -->
<arg name="cfg" />
<!-- Load URDF -->
<include file="$(find nao_dcm_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<!-- Start the database -->
<include file="$(find nao_dcm_moveit_config)/launch/warehouse.launch">
<arg name="moveit_warehouse_database_path" value="moveit_ompl_benchmark_warehouse"/>
</include>
<!-- Start Benchmark Executable -->
<node name="$(anon moveit_benchmark)" pkg="moveit_ros_benchmarks" type="moveit_run_benchmark" args="$(arg cfg) --benchmark-planners" respawn="false" output="screen">
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/kinematics.yaml"/>
<rosparam command="load" file="$(find nao_dcm_moveit_config)/config/ompl_planning.yaml"/>
</node>
</launch>

14
nao_dcm_apps/nao_dcm_moveit_config/launch/sensor_manager.launch.xml Normal file
View File

@ -0,0 +1,14 @@
<launch>
<!-- This file makes it easy to include the settings for sensor managers -->
<!-- Params for the octomap monitor -->
<!-- <param name="octomap_frame" type="string" value="some frame in which the robot moves" /> -->
<param name="octomap_resolution" type="double" value="0.025" />
<param name="max_range" type="double" value="5.0" />
<!-- Load the robot specific sensor manager; this sets the moveit_sensor_manager ROS parameter -->
<arg name="moveit_sensor_manager" default="nao_robot_v4_H25" />
<include file="$(find nao_dcm_moveit_config)/launch/$(arg moveit_sensor_manager)_moveit_sensor_manager.launch.xml" />
</launch>

15
nao_dcm_apps/nao_dcm_moveit_config/launch/setup_assistant.launch Normal file
View File

@ -0,0 +1,15 @@
<!-- Re-launch the MoveIt Setup Assistant with this configuration package already loaded -->
<launch>
<!-- Debug Info -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<!-- Run -->
<node pkg="moveit_setup_assistant" type="moveit_setup_assistant" name="moveit_setup_assistant"
args="--config_pkg=nao_dcm_moveit_config"
launch-prefix="$(arg launch_prefix)"
output="screen" />
</launch>

18
nao_dcm_apps/nao_dcm_moveit_config/launch/trajectory_execution.launch.xml Normal file
View File

@ -0,0 +1,18 @@
<launch>
<!-- This file makes it easy to include the settings for trajectory execution -->
<!-- Flag indicating whether MoveIt! is allowed to load/unload or switch controllers -->
<arg name="moveit_manage_controllers" default="true"/>
<param name="moveit_manage_controllers" value="$(arg moveit_manage_controllers)"/>
<!-- When determining the expected duration of a trajectory, this multiplicative factor is applied to get the allowed duration of execution -->
<param name="allowed_execution_duration_scaling" value="1.2"/> <!-- default 1.2 -->
<!-- Allow more than the expected execution time before triggering a trajectory cancel (applied after scaling) -->
<param name="allowed_goal_duration_margin" value="0.5"/> <!-- default 0.5 -->
<!-- Load the robot specific controller manager; this sets the moveit_controller_manager ROS parameter -->
<arg name="moveit_controller_manager" default="nao_robot_v4_H25" />
<include file="$(find nao_dcm_moveit_config)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />
</launch>

15
nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse.launch Normal file
View File

@ -0,0 +1,15 @@
<launch>
<!-- The path to the database must be specified -->
<arg name="moveit_warehouse_database_path" />
<!-- Load warehouse parameters -->
<include file="$(find nao_dcm_moveit_config)/launch/warehouse_settings.launch.xml" />
<!-- Run the DB server -->
<node name="$(anon mongo_wrapper_ros)" cwd="ROS_HOME" type="mongo_wrapper_ros.py" pkg="warehouse_ros">
<param name="overwrite" value="false"/>
<param name="database_path" value="$(arg moveit_warehouse_database_path)" />
</node>
</launch>

15
nao_dcm_apps/nao_dcm_moveit_config/launch/warehouse_settings.launch.xml Normal file
View File

@ -0,0 +1,15 @@
<launch>
<!-- Set the parameters for the warehouse and run the mongodb server. -->
<!-- The default DB port for moveit (not default MongoDB port to avoid potential conflicts) -->
<arg name="moveit_warehouse_port" default="33829" />
<!-- The default DB host for moveit -->
<arg name="moveit_warehouse_host" default="localhost" />
<!-- Set parameters for the warehouse -->
<param name="warehouse_port" value="$(arg moveit_warehouse_port)"/>
<param name="warehouse_host" value="$(arg moveit_warehouse_host)"/>
<param name="warehouse_exec" value="mongod" />
</launch>

40
nao_dcm_apps/nao_dcm_moveit_config/package.xml Normal file
View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm_moveit_config</name>
<version>0.0.1</version>
<description>
Configuration and launch files for using Aldebaran's Nao (v4) with the MoveIt Motion Planning Framework.
</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>moveit_ros_move_group</run_depend>
<run_depend>moveit_planners_ompl</run_depend>
<run_depend>moveit_ros_visualization</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>xacro</run_depend>
<run_depend>nao_dcm_description</run_depend>
</package>

15
nao_dcm_apps/nao_dcm_rqt_dashboard/CMakeLists.txt Normal file
View File

@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm_rqt_dashboard)
find_package(catkin REQUIRED)
find_package(Qt4 REQUIRED)
catkin_package()
catkin_python_setup()
install(PROGRAMS scripts/nao_dcm_rqt_dashboard
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
install(FILES plugin.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

41
nao_dcm_apps/nao_dcm_rqt_dashboard/package.xml Normal file
View File

@ -0,0 +1,41 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm_rqt_dashboard</name>
<version>0.0.1</version>
<description>
The NaoDCM dashboard is a RQT-based plug-in for visualising data from the Nao and giving easy access
to basic functionalities.
</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>diagnostic_msgs</run_depend>
<run_depend>rospy</run_depend>
<run_depend>rqt_gui</run_depend>
<run_depend>rqt_gui_py</run_depend>
<run_depend>rqt_robot_dashboard</run_depend>
<export>
<rqt_gui plugin="${prefix}/plugin.xml"/>
</export>
</package>

35
nao_dcm_apps/nao_dcm_rqt_dashboard/plugin.xml Normal file
View File

@ -0,0 +1,35 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<library path="src">
<class name="NaoDCMDashboard" type="nao_dcm_rqt_dashboard.nao_dcm_rqt_dashboard.NaoDCMDashboard" base_class_type="rqt_gui_py::Plugin">
<description>
A Python GUI plugin for displaying a dashboard that displays and interacts with low level Nao DCM states.
</description>
<qtgui>
<group>
<label>Robot</label>
<icon type="theme">folder</icon>
<statustip>Plugins related to specific robots.</statustip>
</group>
<group>
<label>NaoDCM</label>
<icon type="theme">folder</icon>
<statustip>Plugins related to the Nao robot.</statustip>
</group>
<label>NaoDCM Dashboard</label>
<icon type="theme">task-past-due</icon>
<statustip>A Python GUI plugin for displaying a dashboard that displays and interacts with low level Nao DCM states.</statustip>
</qtgui>
</class>
</library>

12
nao_dcm_apps/nao_dcm_rqt_dashboard/scripts/nao_dcm_rqt_dashboard Executable file
View File

@ -0,0 +1,12 @@
#!/usr/bin/env python
import sys
import roslib
pkg = 'nao_dcm_rqt_dashboard'
roslib.load_manifest(pkg)
from rqt_gui.main import Main
main = Main()
sys.exit(main.main(sys.argv, standalone=pkg))

9
nao_dcm_apps/nao_dcm_rqt_dashboard/setup.py Executable file
View File

@ -0,0 +1,9 @@
#!/usr/bin/env python
from distutils.core import setup
from catkin_pkg.python_setup import generate_distutils_setup
d = generate_distutils_setup(packages=['nao_dcm_rqt_dashboard'],
package_dir={'': 'src'})
setup(**d)

0
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/__init__.py Executable file
View File

44
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.py Executable file
View File

@ -0,0 +1,44 @@
# Copyright (c) 2014, Konstantinos Chatzilygeroudis
# All rights reserved.
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
# 2. 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.
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard')
import rospy
from rqt_robot_dashboard.widgets import BatteryDashWidget
class NaoDCMBatteryWidget(BatteryDashWidget):
def __init__(self, name='battery'):
icons = []
charge_icons = []
icons.append(['ic-battery-0.svg'])
icons.append(['ic-battery-20.svg'])
icons.append(['ic-battery-40.svg'])
icons.append(['ic-battery-60-green.svg'])
icons.append(['ic-battery-80-green.svg'])
icons.append(['ic-battery-100-green.svg'])
charge_icons.append(['ic-battery-charge-0.svg'])
charge_icons.append(['ic-battery-charge-20.svg'])
charge_icons.append(['ic-battery-charge-40.svg'])
charge_icons.append(['ic-battery-charge-60-green.svg'])
charge_icons.append(['ic-battery-charge-80-green.svg'])
charge_icons.append(['ic-battery-charge-100-green.svg'])
super(NaoDCMBatteryWidget, self).__init__(name=name, icons=icons, charge_icons=charge_icons)
self._pct = 0.0
self._vlt = 0.0
def set_power_state(self, msg):
self._pct = float(msg)
self.update_perc(self._pct)
self.update_time(self._pct)

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/battery.pyc Normal file
View File

Binary file not shown.

123
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.py Normal file
View File

@ -0,0 +1,123 @@
# Copyright (c) 2014, Konstantinos Chatzilygeroudis
# All rights reserved.
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
# 2. 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.
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard')
import rospy
from python_qt_binding.QtCore import Signal, QMutex, QMutexLocker
from python_qt_binding.QtGui import QWidget, QPixmap, QImage, QGraphicsView, QGraphicsScene, QVBoxLayout
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from rqt_robot_dashboard.widgets import IconToolButton
import numpy
class CameraViewDashWidget(IconToolButton):
def __init__(self, context, camera_topic='/image_raw', name='camera'):
self._icons = [['bg-grey.svg', 'ol-play-badge.svg']]
super(CameraViewDashWidget, self).__init__("View"+name, icons=self._icons, icon_paths=None)
self.context = context
self.update_state(0)
self._cam_view = CameraViewWidget(camera_topic,name)
self._cam_view_shown = False
self.clicked.connect(self._show_cam_view)
self._show_mutex = QMutex()
def _show_cam_view(self):
with QMutexLocker(self._show_mutex):
try:
if self._cam_view_shown:
self.context.remove_widget(self._cam_view)
self._cam_view_shown = not self._cam_view_shown
else:
self.context.add_widget(self._cam_view)
self._cam_view_shown = not self._cam_view_shown
except Exception:
self._cam_view_shown = not self._cam_view_shown
self._show_cam_view()
def shutdown_widget(self):
if self._cam_view:
self._cam_view.close()
class CameraViewWidget(QWidget):
def __init__(self, camera_topic='/image_raw', name='camera'):
super(CameraViewWidget,self).__init__()
self._layout = QVBoxLayout()
self.name = name
self._colors = [(238, 34, 116), (68, 134, 252), (236, 228, 46), (102, 224, 18), (242, 156, 6), (240, 64, 10), (196, 30, 250)]
self.setWindowTitle('Camera \''+name+'\' Viewer')
self._cam_view = CameraView(camera_topic)
self._layout.addWidget(self._cam_view)
self.setLayout(self._layout)
def close(self):
if self._cam_view:
self._cam_view.close()
class CameraView(QGraphicsView):
image_changed = Signal()
def __init__(self, camera_topic='/image_raw'):
super(CameraView,self).__init__()
self._scene = QGraphicsScene()
self.bridge = CvBridge()
self._map_item = 0
self.image_changed.connect(self._update)
self._sub = rospy.Subscriber(camera_topic,Image,self.callback)
self.setScene(self._scene)
def callback(self, msg):
self.w = msg.width
self.h = msg.height
a = self.bridge.imgmsg_to_cv(msg, "rgb8")
a = numpy.array(a)
image = QImage(a, self.w, self.h, QImage.Format_RGB888)
self._map = image
self._scene.setSceneRect(0,0,self.w,self.h)
self.image_changed.emit()
def _update(self):
if self._map_item:
self._scene.removeItem(self._map_item)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
self.centerOn(self._map_item)
self.show()
def _mirror(self, item):
item.scale(-1, 1)
item.translate(-self.w, 0)
def close(self):
if self._sub:
self._sub.unregister()
super(CameraView, self).close()

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/camera.pyc Normal file
View File

Binary file not shown.

81
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.py Executable file
View File

@ -0,0 +1,81 @@
# Copyright (c) 2014, Konstantinos Chatzilygeroudis
# All rights reserved.
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
# 2. 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.
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard')
import rospy
import diagnostic_msgs
from rqt_robot_dashboard.dashboard import Dashboard
from rqt_robot_dashboard.widgets import MonitorDashWidget, ConsoleDashWidget, MenuDashWidget, IconToolButton, NavViewDashWidget
from QtGui import QMessageBox, QAction
from python_qt_binding.QtCore import QSize
from .battery import NaoDCMBatteryWidget
from .camera import CameraViewDashWidget
from .stiffness import NaoDCMStiffnessWidget
import rospkg
import os.path
rp = rospkg.RosPack()
class NaoDCMDashboard(Dashboard):
def setup(self, context):
self._dashboard_message = None
self._last_dashboard_message_time = 0.0
self.name = "NaoDCM Dashboard"
rospy.sleep(0.5)
self.motor_bat = NaoDCMBatteryWidget("Battery")
self.console = ConsoleDashWidget(self.context)
self.monitor = MonitorDashWidget(self.context)
self.monitor._show_monitor()
self.nav = NavViewDashWidget(self.context)
self.cam = CameraViewDashWidget(self.context, 'nao_dcm/CameraTop/image_raw', 'CameraTop')
self.cam._show_cam_view()
self.camBot = CameraViewDashWidget(self.context, 'nao_dcm/CameraBottom/image_raw', 'CameraBottom')
self.camBot._show_cam_view()
self.stiff = NaoDCMStiffnessWidget("Stiffness")
# This is what gets dashboard_callback going eagerly
self._dashboard_agg_sub = rospy.Subscriber('diagnostics_agg', diagnostic_msgs.msg.DiagnosticArray, self.dashboard_callback)
def get_widgets(self):
return [[self.monitor, self.console],[self.motor_bat],[self.nav],[self.cam, self.camBot],[self.stiff]]
def dashboard_callback(self, msg):
self._dashboard_message = msg
self._last_dashboard_message_time = rospy.get_time()
battery_status = {} # Used to store Battery status info
for status in msg.status:
if status.name == "/Nao/Battery/Battery":
for value in status.values:
battery_status[value.key]=value.value
# If battery diagnostics were found, calculate percentages and stuff
if (battery_status):
self.motor_bat.set_power_state(battery_status['Battery Charge'])
def shutdown_dashboard(self):
self._dashboard_agg_sub.unregister()
def save_settings(self, plugin_settings, instance_settings):
self.console.save_settings(plugin_settings, instance_settings)
self.monitor.save_settings(plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings):
self.console.restore_settings(plugin_settings, instance_settings)
self.monitor.restore_settings(plugin_settings, instance_settings)

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/nao_dcm_rqt_dashboard.pyc Normal file
View File

Binary file not shown.

50
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.py Normal file
View File

@ -0,0 +1,50 @@
# Copyright (c) 2014, Konstantinos Chatzilygeroudis
# All rights reserved.
# Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
# 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
# 2. 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.
# 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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.
import roslib;roslib.load_manifest('nao_dcm_rqt_dashboard')
import rospy
from nao_dcm_msgs.srv import BoolService, BoolServiceRequest
from std_msgs.msg import Float32
from rqt_robot_dashboard.widgets import IconToolButton
class NaoDCMStiffnessWidget(IconToolButton):
def __init__(self, name='stiffness'):
self.name = name
icons = []
icons.append(['bg-red.svg','ic-motors.svg'])
icons.append(['bg-green.svg','ic-motors.svg'])
super(NaoDCMStiffnessWidget,self).__init__(name=name,icons=icons)
self.update_state(0)
self.stiffness = 1.0
self.clicked.connect(self.changeStiffness)
self._sub = rospy.Subscriber('/nao_dcm/stiffnesses',Float32,self.callback)
def changeStiffness(self):
stiff = rospy.ServiceProxy('/nao_dcm/Stiffnesses/Enable',BoolService)
req = BoolServiceRequest()
if(self.stiffness==1.0):
req.enable = False
else:
req.enable = True
self.stiffness = 1.0-self.stiffness
stiff(req)
def callback(self, msg):
self.stiffness = msg.data
if(msg.data == 1.0):
self.update_state(0)
else:
self.update_state(1)

BIN
nao_dcm_apps/nao_dcm_rqt_dashboard/src/nao_dcm_rqt_dashboard/stiffness.pyc Normal file
View File

Binary file not shown.

14
nao_dcm_common/nao_dcm_control/CMakeLists.txt Normal file
View File

@ -0,0 +1,14 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm_control)
## Find catkin
find_package(catkin REQUIRED)
##Needed for ros packages
catkin_package()
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

112
nao_dcm_common/nao_dcm_control/config/nao_position_control.yaml Normal file
View File

@ -0,0 +1,112 @@
nao_dcm:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Controllers ------------------------------------------------
HeadYaw_position_controller:
type: position_controllers/JointPositionController
joint: HeadYaw
HeadPitch_position_controller:
type: position_controllers/JointPositionController
joint: HeadPitch
LShoulderPitch_position_controller:
type: position_controllers/JointPositionController
joint: LShoulderPitch
LElbowYaw_position_controller:
type: position_controllers/JointPositionController
joint: LElbowYaw
LWristYaw_position_controller:
type: position_controllers/JointPositionController
joint: LWristYaw
LHand_position_controller:
type: position_controllers/JointPositionController
joint: LHand
LShoulderRoll_position_controller:
type: position_controllers/JointPositionController
joint: LShoulderRoll
LElbowRoll_position_controller:
type: position_controllers/JointPositionController
joint: LElbowRoll
RShoulderPitch_position_controller:
type: position_controllers/JointPositionController
joint: RShoulderPitch
RElbowYaw_position_controller:
type: position_controllers/JointPositionController
joint: RElbowYaw
RWristYaw_position_controller:
type: position_controllers/JointPositionController
joint: RWristYaw
RHand_position_controller:
type: position_controllers/JointPositionController
joint: RHand
RShoulderRoll_position_controller:
type: position_controllers/JointPositionController
joint: RShoulderRoll
RElbowRoll_position_controller:
type: position_controllers/JointPositionController
joint: RElbowRoll
LHipYawPitch_position_controller:
type: position_controllers/JointPositionController
joint: LHipYawPitch
LHipRoll_position_controller:
type: position_controllers/JointPositionController
joint: LHipRoll
LHipPitch_position_controller:
type: position_controllers/JointPositionController
joint: LHipPitch
LKneePitch_position_controller:
type: position_controllers/JointPositionController
joint: LKneePitch
LAnklePitch_position_controller:
type: position_controllers/JointPositionController
joint: LAnklePitch
LAnkleRoll_position_controller:
type: position_controllers/JointPositionController
joint: LAnkleRoll
RHipYawPitch_position_controller:
type: position_controllers/JointPositionController
joint: RHipYawPitch
RHipRoll_position_controller:
type: position_controllers/JointPositionController
joint: RHipRoll
RHipPitch_position_controller:
type: position_controllers/JointPositionController
joint: RHipPitch
RKneePitch_position_controller:
type: position_controllers/JointPositionController
joint: RKneePitch
RAnklePitch_position_controller:
type: position_controllers/JointPositionController
joint: RAnklePitch
RAnkleRoll_position_controller:
type: position_controllers/JointPositionController
joint: RAnkleRoll

61
nao_dcm_common/nao_dcm_control/config/nao_trajectory_control.yaml Normal file
View File

@ -0,0 +1,61 @@
nao_dcm:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Controllers ------------------------------------------------
Head_controller:
type: position_controllers/JointTrajectoryController
joints:
- HeadYaw
- HeadPitch
LeftArm_controller:
type: position_controllers/JointTrajectoryController
joints:
- LShoulderPitch
- LShoulderRoll
- LElbowYaw
- LElbowRoll
RightArm_controller:
type: position_controllers/JointTrajectoryController
joints:
- RShoulderPitch
- RShoulderRoll
- RElbowYaw
- RElbowRoll
LeftHand_controller:
type: position_controllers/JointTrajectoryController
joints:
- LWristYaw
- LHand
RightHand_controller:
type: position_controllers/JointTrajectoryController
joints:
- RWristYaw
- RHand
Pelvis_controller:
type: position_controllers/JointTrajectoryController
joints:
- LHipYawPitch
LeftLeg_controller:
type: position_controllers/JointTrajectoryController
joints:
- LHipRoll
- LHipPitch
- LKneePitch
- LAnklePitch
- LAnkleRoll
RightLeg_controller:
type: position_controllers/JointTrajectoryController
joints:
- RHipRoll
- RHipPitch
- RKneePitch
- RAnklePitch
- RAnkleRoll
Head_controller:
type: position_controllers/JointTrajectoryController
joints:
- HeadYaw
- HeadPitch

51
nao_dcm_common/nao_dcm_control/config/nao_trajectory_control_H21.yaml Normal file
View File

@ -0,0 +1,51 @@
nao_dcm:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Controllers ------------------------------------------------
Head_controller:
type: position_controllers/JointTrajectoryController
joints:
- HeadYaw
- HeadPitch
LeftArm_controller:
type: position_controllers/JointTrajectoryController
joints:
- LShoulderPitch
- LShoulderRoll
- LElbowYaw
- LElbowRoll
RightArm_controller:
type: position_controllers/JointTrajectoryController
joints:
- RShoulderPitch
- RShoulderRoll
- RElbowYaw
- RElbowRoll
Pelvis_controller:
type: position_controllers/JointTrajectoryController
joints:
- LHipYawPitch
LeftLeg_controller:
type: position_controllers/JointTrajectoryController
joints:
- LHipRoll
- LHipPitch
- LKneePitch
- LAnklePitch
- LAnkleRoll
RightLeg_controller:
type: position_controllers/JointTrajectoryController
joints:
- RHipRoll
- RHipPitch
- RKneePitch
- RAnklePitch
- RAnkleRoll
Head_controller:
type: position_controllers/JointTrajectoryController
joints:
- HeadYaw
- HeadPitch

17
nao_dcm_common/nao_dcm_control/launch/nao_dcm_position_control.launch Normal file
View File

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_control)/config/nao_position_control.yaml" command="load"/>
<!-- load the controllers -->
<node name="nao_controller" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/nao_dcm" args="/nao_dcm/HeadYaw_position_controller /nao_dcm/HeadPitch_position_controller /nao_dcm/LShoulderPitch_position_controller /nao_dcm/LElbowYaw_position_controller /nao_dcm/LWristYaw_position_controller /nao_dcm/LShoulderRoll_position_controller /nao_dcm/LElbowRoll_position_controller /nao_dcm/RShoulderPitch_position_controller /nao_dcm/RElbowYaw_position_controller /nao_dcm/RWristYaw_position_controller /nao_dcm/RShoulderRoll_position_controller /nao_dcm/RElbowRoll_position_controller /nao_dcm/LHipYawPitch_position_controller /nao_dcm/LHipRoll_position_controller /nao_dcm/LHipPitch_position_controller /nao_dcm/LKneePitch_position_controller /nao_dcm/LAnklePitch_position_controller /nao_dcm/LAnkleRoll_position_controller /nao_dcm/RHipYawPitch_position_controller /nao_dcm/RHipRoll_position_controller /nao_dcm/RHipPitch_position_controller /nao_dcm/RKneePitch_position_controller /nao_dcm/RAnklePitch_position_controller /nao_dcm/RAnkleRoll_position_controller /nao_dcm/joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/nao_dcm/joint_states" />
</node>
</launch>

17
nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H21.launch Normal file
View File

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_control)/config/nao_trajectory_control_H21.yaml" command="load"/>
<!-- load the controllers -->
<node name="nao_trajectory_controller" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/nao_dcm" args="/nao_dcm/RightArm_controller /nao_dcm/LeftArm_controller /nao_dcm/Head_controller /nao_dcm/Pelvis_controller /nao_dcm/LeftLeg_controller /nao_dcm/RightLeg_controller /nao_dcm/joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/nao_dcm/joint_states" />
</node>
</launch>

17
nao_dcm_common/nao_dcm_control/launch/nao_dcm_trajectory_control_H25.launch Normal file
View File

@ -0,0 +1,17 @@
<?xml version="1.0"?>
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find nao_dcm_control)/config/nao_trajectory_control.yaml" command="load"/>
<!-- load the controllers -->
<node name="nao_trajectory_controller" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/nao_dcm" args="/nao_dcm/RightArm_controller /nao_dcm/RightHand_controller /nao_dcm/LeftArm_controller /nao_dcm/LeftHand_controller /nao_dcm/Head_controller /nao_dcm/Pelvis_controller /nao_dcm/LeftLeg_controller /nao_dcm/RightLeg_controller /nao_dcm/joint_state_controller"/>
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
respawn="false" output="screen">
<remap from="/joint_states" to="/nao_dcm/joint_states" />
</node>
</launch>

37
nao_dcm_common/nao_dcm_control/package.xml Normal file
View File

@ -0,0 +1,37 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm_control</name>
<version>0.0.1</version>
<description>Control for Aldebaran's NAO robot (v4)</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>ros_control</run_depend>
<run_depend>ros_controllers</run_depend>
<export>
</export>
</package>

18
nao_dcm_common/nao_dcm_description/CMakeLists.txt Normal file
View File

@ -0,0 +1,18 @@
cmake_minimum_required(VERSION 2.8.3)
project(nao_dcm_description)
## Find catkin
find_package(catkin REQUIRED)
##Needed for ros packages
catkin_package(CATKIN_DEPENDS xacro)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY urdf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
install(DIRECTORY config
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

262
nao_dcm_common/nao_dcm_description/config/nao_dcm.rviz Normal file
View File

@ -0,0 +1,262 @@
Panels:
- Class: rviz/Displays
Help Height: 81
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /RobotModel1
Splitter Ratio: 0.5
Tree Height: 720
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: ""
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.03
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Class: rviz/RobotModel
Collision Enabled: false
Enabled: true
Links:
All Links Enabled: true
CameraBottom:
Alpha: 1
Show Axes: false
Show Trail: false
CameraTop:
Alpha: 1
Show Axes: false
Show Trail: false
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
LElbow:
Alpha: 1
Show Axes: false
Show Trail: false
LElbowDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LHip:
Alpha: 1
Show Axes: false
Show Trail: false
LHipDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LHipYaw:
Alpha: 1
Show Axes: false
Show Trail: false
LKnee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
LShoulder:
Alpha: 1
Show Axes: false
Show Trail: false
LShoulderDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Link Tree Style: Links in Alphabetic Order
RElbow:
Alpha: 1
Show Axes: false
Show Trail: false
RElbowDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RHip:
Alpha: 1
Show Axes: false
Show Trail: false
RHipDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RHipYaw:
Alpha: 1
Show Axes: false
Show Trail: false
RKnee:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
RShoulder:
Alpha: 1
Show Axes: false
Show Trail: false
RShoulderDummy:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
SonarLeft:
Alpha: 1
Show Axes: false
Show Trail: false
SonarRight:
Alpha: 1
Show Axes: false
Show Trail: false
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
gaze:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_ankle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_sole:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
l_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
neck:
Alpha: 1
Show Axes: false
Show Trail: false
r_ankle:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_sole:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
r_wrist:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
torso:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
Robot Description: robot_description
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 1.44466
Enable Stereo Rendering:
Stereo Eye Separation: 0.06
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Name: Current View
Near Clip Distance: 0.01
Pitch: 0.355398
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 0.755394
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1031
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000013c00000362fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006700fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004300000362000000e300fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000362fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004300000362000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000023a00fffffffb0000000800540069006d00650100000000000004500000000000000000000005290000036200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: -2
Y: 25

12
nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H21.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<include file="$(find nao_dcm_description)/launch/nao_dcm_H21_urdf.launch" />
<arg name="model" />
<arg name="gui" default="true" />
<param name="use_gui" value="$(arg gui)" />
<!-- Start state publishers -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- Start RVIZ -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nao_dcm_description)/config/nao_dcm.rviz" />
</launch>

12
nao_dcm_common/nao_dcm_description/launch/display_nao_dcm_H25.launch Normal file
View File

@ -0,0 +1,12 @@
<?xml version="1.0"?>
<launch>
<include file="$(find nao_dcm_description)/launch/nao_dcm_H25_urdf.launch" />
<arg name="model" />
<arg name="gui" default="true" />
<param name="use_gui" value="$(arg gui)" />
<!-- Start state publishers -->
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
<!-- Start RVIZ -->
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find nao_dcm_description)/config/nao_dcm.rviz" />
</launch>

5
nao_dcm_common/nao_dcm_description/launch/nao_dcm_H21_urdf.launch Normal file
View File

@ -0,0 +1,5 @@
<?xml version="1.0"?>
<launch>
<!-- Load Nao Robot parameter file -->
<param name="robot_description" command="$(find xacro)/xacro.py $(find nao_dcm_description)/urdf/nao_dcm_H21.xacro" />
</launch>

5
nao_dcm_common/nao_dcm_description/launch/nao_dcm_H25_urdf.launch Normal file
View File

@ -0,0 +1,5 @@
<?xml version="1.0"?>
<launch>
<!-- Load Nao Robot parameter file -->
<param name="robot_description" command="$(find xacro)/xacro.py $(find nao_dcm_description)/urdf/nao_dcm_H25.xacro" />
</launch>

39
nao_dcm_common/nao_dcm_description/package.xml Normal file
View File

@ -0,0 +1,39 @@
<?xml version="1.0"?>
<!-- Copyright (c) 2014, Konstantinos Chatzilygeroudis
All rights reserved.
Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
2. 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.
3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 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. -->
<package>
<name>nao_dcm_description</name>
<version>0.0.1</version>
<description>URDF for Aldebaran's NAO robot (v4)</description>
<maintainer email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</maintainer>
<license>BSD</license>
<url type="repository">https://github.com/costashatz/nao_dcm</url>
<url type="bugtracker">https://github.com/costashatz/nao_dcm/issues</url>
<author email="costashatz@gmail.com">Konstantinos Chatzilygeroudis</author>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>robot_state_publisher</run_depend>
<run_depend>joint_state_publisher</run_depend>
<run_depend>rviz</run_depend>
<run_depend>xacro</run_depend>
<run_depend>urdf</run_depend>
<export>
</export>
</package>

14
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21.xacro Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<robot name="nao_robot_v4_H21" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- XACRO PROPERTIES -->
<xacro:property name="PI" value="3.14159265359" />
<xacro:property name="PI_2" value="1.57079632679" />
<xacro:property name="scale" value="0.1" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_visuals_collisions.xacro" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_gazebo.xacro" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H21_structure.xacro" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H21_control.xacro" />
</robot>

124
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_control.xacro Normal file
View File

@ -0,0 +1,124 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- HEAD -->
<transmission name="HeadYaw_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="HeadYaw"/>
<actuator name="HeadYaw_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>150.27</mechanicalReduction>
</actuator>
</transmission>
<transmission name="HeadPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="HeadPitch"/>
<actuator name="HeadPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>173.22</mechanicalReduction>
</actuator>
</transmission>
<!-- ARMS -->
<!-- XACRO -->
<xacro:macro name="arm_joints_transmissions" params="side">
<transmission name="${side}ShoulderPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ShoulderPitch"/>
<actuator name="${side}ShoulderPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>150.27</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}ElbowYaw_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ElbowYaw"/>
<actuator name="${side}ElbowYaw_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>150.27</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}ShoulderRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ShoulderRoll"/>
<actuator name="${side}ShoulderRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>173.22</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}ElbowRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ElbowRoll"/>
<actuator name="${side}ElbowRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>173.22</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:arm_joints_transmissions side="L"/>
<xacro:arm_joints_transmissions side="R"/>
<!-- LEGS -->
<!-- XACRO -->
<xacro:macro name="leg_joints_transmissions" params="side">
<transmission name="${side}HipYawPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}HipYawPitch"/>
<actuator name="${side}HipYawPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>201.3</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}HipRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}HipRoll"/>
<actuator name="${side}HipRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>201.3</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}HipPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}HipPitch"/>
<actuator name="${side}HipPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>130.85</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}KneePitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}KneePitch"/>
<actuator name="${side}KneePitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>130.85</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}AnklePitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}AnklePitch"/>
<actuator name="${side}AnklePitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>130.85</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}AnkleRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}AnkleRoll"/>
<actuator name="${side}AnkleRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>201.3</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:leg_joints_transmissions side="L"/>
<xacro:leg_joints_transmissions side="R"/>
</robot>

412
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H21_structure.xacro Normal file
View File

@ -0,0 +1,412 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- DUMMY ROOT LINK -->
<link name="base_link"/>
<joint name="fixed-base" type="fixed">
<parent link="base_link"/>
<child link="torso"/>
</joint>
<!-- TORSO -->
<link name="torso">
<xacro:visuals_collisions_torso/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00413 0.00009 0.04342"/>
<mass value="1.04956"/>
<inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.0048013591" iyz="-0.00002707934" izz="0.00161030006"/>
</inertial>
</link>
<!-- SONARS -->
<!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
<!-- <joint name="RightSonarTransmitter" type="fixed">
<parent link="torso"/>
<child link="SonarRightTransmitter"/>
<origin xyz="0.0537 -0.0341 0.0698" rpy="0.0 -0.1745 -0.3490"/>
</joint>
<link name="SonarRightTransmitter"/>
<joint name="LeftSonarTransmitter" type="fixed">
<parent link="torso"/>
<child link="SonarLeftTransmitter"/>
<origin xyz="0.0537 0.0341 0.0698" rpy="0.0 -0.1745 0.3490"/>
</joint>
<link name="SonarLeftTransmitter"/> -->
<joint name="RightSonar" type="fixed">
<parent link="torso"/>
<child link="SonarRight"/>
<origin xyz="0.0477 -0.0416 0.0509" rpy="0 0.2618 -0.4363"/>
</joint>
<link name="SonarRight"/>
<joint name="LeftSonar" type="fixed">
<parent link="torso"/>
<child link="SonarLeft"/>
<origin xyz="0.0477 0.0416 0.0509" rpy="0 0.2618 0.4363"/>
</joint>
<link name="SonarLeft"/>
<!-- TORSO-HEAD JOINT -->
<joint name="HeadYaw" type="revolute">
<parent link="torso"/>
<child link="neck"/>
<limit effort="0.62" lower="-2.0857" upper="2.0857" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0.1265"/>
<axis xyz="0 0 1"/>
</joint>
<!-- HEAD -->
<link name="neck">
<xacro:visuals_collisions_head_yaw/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00001 0.00014 -0.02742"/>
<mass value="0.06442"/>
<inertia ixx="0.00007499295" ixy="0.00000000157" ixz="-0.00000001834" iyy="0.00007599995" iyz="-0.00000005295" izz="0.00000553373"/>
</inertial>
</link>
<joint name="HeadPitch" type="revolute">
<parent link="neck"/>
<child link="gaze"/>
<limit effort="0.62" lower="-0.6720" upper="0.5149" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="gaze">
<xacro:visuals_collisions_head_pitch/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00112 0.00003 0.05258"/>
<mass value="0.6533"/>
<inertia ixx="0.00263129518" ixy="0.00000878814" ixz="0.00004098466" iyy="0.00249112488" iyz="-0.00002995792" izz="0.00098573565"/>
</inertial>
</link>
<!-- CAMERAS -->
<joint name="TopCamera" type="fixed">
<parent link="gaze"/>
<child link="CameraTop"/>
<origin xyz="0.05871 0 0.06364" rpy="0 0.0209 0"/>
</joint>
<link name="CameraTop"/>
<joint name="BottomCamera" type="fixed">
<parent link="gaze"/>
<child link="CameraBottom"/>
<origin xyz="0.05071 0 0.01774" rpy="0 0.6929 0"/>
</joint>
<link name="CameraBottom"/>
<!-- ARM LINKS/JOINTS -->
<!-- Shoulder Roll Links are different for the 2 arms -->
<link name="LShoulderDummy">
<xacro:visuals_collisions_shoulder_roll side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
<mass value="0.15777"/>
<inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
</inertial>
</link>
<link name="RShoulderDummy">
<xacro:visuals_collisions_shoulder_roll side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
<mass value="0.15777"/>
<inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
</inertial>
</link>
<!-- XACRO -->
<xacro:macro name="arm_links_joints" params="side reflect">
<joint name="${side}ShoulderPitch" type="revolute">
<parent link="torso"/>
<child link="${side}Shoulder"/>
<limit effort="0.62" lower="-2.0857" upper="2.0857" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 ${reflect*0.098} 0.1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${side}Shoulder">
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00165 ${reflect*(-0.02663)} 0.00014"/>
<mass value="0.07504"/>
<inertia ixx="0.00008428430" ixy="${reflect*-0.00000202802}" ixz="0.00000002338" iyy="0.00001415561" iyz="${reflect*-0.00000001972}" izz="0.00008641949"/>
</inertial>
</link>
<joint name="${side}ElbowYaw" type="revolute">
<parent link="${side}ShoulderDummy"/>
<child link="${side}Elbow"/>
<limit effort="0.62" lower="-2.0857" upper="2.0857" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0.105 ${reflect*0.015} 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="${side}Elbow">
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
<mass value="0.06483"/>
<inertia ixx="0.00000559715" ixy="0.00000000421" ixz="0.00000004319" iyy="0.00007543312" iyz="-0.00000000184" izz="0.00007644339"/>
</inertial>
</link>
<link name="${side}ElbowDummy">
<xacro:visuals_collisions_elbow_roll side="${side}"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
<mass value="0.07761"/>
<inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
</inertial>
</link>
</xacro:macro>
<xacro:arm_links_joints reflect="1" side="L"/>
<xacro:arm_links_joints reflect="-1" side="R"/>
<!-- JOINTS/LINKS THAT NEED TO BE NAMED FOR REP-120 -->
<joint name="LWristYaw" type="fixed">
<parent link="LElbowDummy"/>
<child link="l_wrist"/>
<origin rpy="0 0 0" xyz="0.05595 0 0"/>
</joint>
<link name="l_wrist">
<xacro:visuals_collisions_wrist_yaw reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.06530 0.00114 0.00051"/>
<mass value="0.18415"/>
<inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
</inertial>
</link>
<joint name="RWristYaw" type="fixed">
<parent link="RElbowDummy"/>
<child link="r_wrist"/>
<origin rpy="0 0 0" xyz="0.05595 0 0"/>
</joint>
<link name="r_wrist">
<xacro:visuals_collisions_wrist_yaw reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.06530 0.00114 0.00051"/>
<mass value="0.18415"/>
<inertia ixx="0.00007810423" ixy="-0.00001233383" ixz="0.00000744169" iyy="0.00112084378" iyz="-0.00000054551" izz="0.00112214306"/>
</inertial>
</link>
<!-- JOINTS THAT ARE NOT IDENTICAL TO 2 ENDS -->
<joint name="LShoulderRoll" type="revolute">
<parent link="LShoulder"/>
<child link="LShoulderDummy"/>
<limit effort="0.62" lower="-0.3142" upper="1.3265" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="RShoulderRoll" type="revolute">
<parent link="RShoulder"/>
<child link="RShoulderDummy"/>
<limit effort="0.62" lower="-1.3265" upper="0.3142" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="LElbowRoll" type="revolute">
<parent link="LElbow"/>
<child link="LElbowDummy"/>
<limit effort="0.62" lower="-1.5446" upper="-0.0349" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="RElbowRoll" type="revolute">
<parent link="RElbow"/>
<child link="RElbowDummy"/>
<limit effort="0.62" lower="0.0349" upper="1.5446" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- END OF ARM LINKS/JOINTS -->
<!-- LEFT LEG LINKS/JOINTS -->
<joint name="LHipYawPitch" type="revolute">
<parent link="base_link"/>
<child link="LHipYaw"/>
<limit effort="1.61" lower="-1.145303" upper="0.740810" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
<axis xyz="0 0.707106 -0.707106"/>
</joint>
<link name="LHipYaw">
<xacro:visuals_collisions_hip_yaw_pitch reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
<mass value="0.06981"/>
<inertia ixx="0.00008150233" ixy="-0.00000499449" ixz="0.00001274817" iyy="0.00010132555" iyz="0.00002345474" izz="0.000062626363"/>
</inertial>
</link>
<joint name="LHipRoll" type="revolute">
<parent link="LHipYaw"/>
<child link="LHip"/>
<limit effort="1.61" lower="-0.379472" upper="0.790477" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="LHip">
<xacro:visuals_collisions_hip_roll side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
<mass value="0.13053"/>
<inertia ixx="0.00002758354" ixy="-0.00000002233" ixz="-0.00000408164" iyy="0.00009827055" iyz="-0.00000000419" izz="0.00008809973"/>
</inertial>
</link>
<joint name="LHipPitch" type="revolute">
<parent link="LHip"/>
<child link="LHipDummy"/>
<limit effort="1.61" lower="-1.535889" upper="0.484090" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="LHipDummy">
<xacro:visuals_collisions_hip_pitch reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
<mass value="0.38968"/>
<inertia ixx="0.00163671962" ixy="0.00000092451" ixz="0.00008530668" iyy="0.00159107278" iyz="0.00003836160" izz="0.00030374342"/>
</inertial>
</link>
<joint name="LKneePitch" type="revolute">
<parent link="LHipDummy"/>
<child link="LKnee"/>
<limit effort="1.61" lower="-0.092346" upper="2.112528" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="LKnee">
<xacro:visuals_collisions_knee_pitch reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
<mass value="0.29142"/>
<inertia ixx="0.00118207967" ixy="0.00000063362" ixz="0.00003649697" iyy="0.00112865226" iyz="0.00003949523" izz="0.00019322744"/>
</inertial>
</link>
<joint name="LAnklePitch" type="revolute">
<parent link="LKnee"/>
<child link="l_ankle"/>
<limit effort="1.61" lower="-1.189516" upper="0.922747" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1029"/>
<axis xyz="0 1 0"/>
</joint>
<link name="l_ankle">
<xacro:visuals_collisions_ankle_pitch side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
<mass value="0.13416"/>
<inertia ixx="0.00003850978" ixy="-0.00000002634" ixz="0.00000386194" iyy="0.00007426526" iyz="0.00000001834" izz="0.00005486540"/>
</inertial>
</link>
<joint name="LAnkleRoll" type="revolute">
<parent link="l_ankle"/>
<child link="l_sole"/>
<limit effort="1.61" lower="-0.397880" upper="0.769001" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="l_sole">
<xacro:visuals_collisions_ankle_roll side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
<mass value="0.16184"/>
<inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
</inertial>
</link>
<!-- RIGHT LEG LINKS/JOINTS -->
<joint name="RHipYawPitch" type="revolute">
<parent link="base_link"/>
<child link="RHipYaw"/>
<limit effort="1.61" lower="-1.145303" upper="0.740810" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
<axis xyz="0 0.707106 0.707106"/>
<mimic joint="LHipYawPitch" multiplier="1" offset="0"/>
</joint>
<link name="RHipYaw">
<xacro:visuals_collisions_hip_yaw_pitch reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00766 0.01200 0.02716"/>
<mass value="0.07118"/>
<inertia ixx="0.00008997195" ixy="0.00000500219" ixz="0.00001273525" iyy="0.00010552611" iyz="-0.00002770080" izz="0.00006688724"/>
</inertial>
</link>
<joint name="RHipRoll" type="revolute">
<parent link="RHipYaw"/>
<child link="RHip"/>
<limit effort="1.61" lower="-0.790477" upper="0.379472" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="RHip">
<xacro:visuals_collisions_hip_roll side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00516"/>
<mass value="0.13053"/>
<inertia ixx="0.00002758654" ixy="-0.00000001919" ixz="-0.00000410822" iyy="0.00009826996" iyz="0.00000000251" izz="0.00008810332"/>
</inertial>
</link>
<joint name="RHipPitch" type="revolute">
<parent link="RHip"/>
<child link="RHipDummy"/>
<limit effort="1.61" lower="-1.535889" upper="0.484090" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="RHipDummy">
<xacro:visuals_collisions_hip_pitch reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00139 -0.00225 -0.05374"/>
<mass value="0.38976"/>
<inertia ixx="0.00163748205" ixy="-0.00000083954" ixz="0.00008588301" iyy="0.00159221403" iyz="-0.00003917626" izz="0.00030397824"/>
</inertial>
</link>
<joint name="RKneePitch" type="revolute">
<parent link="RHipDummy"/>
<child link="RKnee"/>
<limit effort="1.61" lower="-0.103083" upper="2.120198" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="RKnee">
<xacro:visuals_collisions_knee_pitch reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00394 -0.00221 -0.04938"/>
<mass value="0.29163"/>
<inertia ixx="0.00118282956" ixy="-0.00000089650" ixz="0.00002799690" iyy="0.00112827851" iyz="-0.00003847604" izz="0.00019145277"/>
</inertial>
</link>
<joint name="RAnklePitch" type="revolute">
<parent link="RKnee"/>
<child link="r_ankle"/>
<limit effort="1.61" lower="-1.186448" upper="0.932056" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1029"/>
<axis xyz="0 1 0"/>
</joint>
<link name="r_ankle">
<xacro:visuals_collisions_ankle_pitch side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00045 -0.00030 0.00684"/>
<mass value="0.13415"/>
<inertia ixx="0.00003850813" ixy="0.00000006434" ixz="0.00000387466" iyy="0.00007431082" iyz="-0.00000000458" izz="0.00005491312"/>
</inertial>
</link>
<joint name="RAnkleRoll" type="revolute">
<parent link="r_ankle"/>
<child link="r_sole"/>
<limit effort="1.61" lower="-0.768992" upper="0.397935" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="r_sole">
<xacro:visuals_collisions_ankle_roll side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
<mass value="0.16171"/>
<inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
</inertial>
</link>
<!-- END OF LEG LINKS/JOINTS -->
</robot>

14
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25.xacro Normal file
View File

@ -0,0 +1,14 @@
<?xml version="1.0"?>
<robot name="nao_robot_v4_H25" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- XACRO PROPERTIES -->
<xacro:property name="PI" value="3.14159265359" />
<xacro:property name="PI_2" value="1.57079632679" />
<xacro:property name="scale" value="0.1" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_visuals_collisions.xacro" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_gazebo.xacro" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H25_structure.xacro" />
<xacro:include filename="$(find nao_dcm_description)/urdf/nao_dcm_H25_control.xacro" />
</robot>

142
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_control.xacro Normal file
View File

@ -0,0 +1,142 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- HEAD -->
<transmission name="HeadYaw_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="HeadYaw"/>
<actuator name="HeadYaw_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>150.27</mechanicalReduction>
</actuator>
</transmission>
<transmission name="HeadPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="HeadPitch"/>
<actuator name="HeadPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>173.22</mechanicalReduction>
</actuator>
</transmission>
<!-- ARMS -->
<!-- XACRO -->
<xacro:macro name="arm_joints_transmissions" params="side">
<transmission name="${side}ShoulderPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ShoulderPitch"/>
<actuator name="${side}ShoulderPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>150.27</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}ElbowYaw_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ElbowYaw"/>
<actuator name="${side}ElbowYaw_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>150.27</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}WristYaw_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}WristYaw"/>
<actuator name="${side}WristYaw_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>50.61</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}Hand_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}Hand"/>
<actuator name="${side}Hand_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>36.24</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}ShoulderRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ShoulderRoll"/>
<actuator name="${side}ShoulderRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>173.22</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}ElbowRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}ElbowRoll"/>
<actuator name="${side}ElbowRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>173.22</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:arm_joints_transmissions side="L"/>
<xacro:arm_joints_transmissions side="R"/>
<!-- LEGS -->
<!-- XACRO -->
<xacro:macro name="leg_joints_transmissions" params="side">
<transmission name="${side}HipYawPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}HipYawPitch"/>
<actuator name="${side}HipYawPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>201.3</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}HipRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}HipRoll"/>
<actuator name="${side}HipRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>201.3</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}HipPitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}HipPitch"/>
<actuator name="${side}HipPitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>130.85</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}KneePitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}KneePitch"/>
<actuator name="${side}KneePitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>130.85</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}AnklePitch_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}AnklePitch"/>
<actuator name="${side}AnklePitch_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>130.85</mechanicalReduction>
</actuator>
</transmission>
<transmission name="${side}AnkleRoll_Transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${side}AnkleRoll"/>
<actuator name="${side}AnkleRoll_Motor">
<!-- Dummy Values -->
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>201.3</mechanicalReduction>
</actuator>
</transmission>
</xacro:macro>
<xacro:leg_joints_transmissions side="L"/>
<xacro:leg_joints_transmissions side="R"/>
</robot>

430
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_H25_structure.xacro Normal file
View File

@ -0,0 +1,430 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- DUMMY ROOT LINK -->
<link name="base_link"/>
<joint name="fixed-base" type="fixed">
<parent link="base_link"/>
<child link="torso"/>
</joint>
<!-- TORSO -->
<link name="torso">
<xacro:visuals_collisions_torso/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00413 0.00009 0.04342"/>
<mass value="1.04956"/>
<inertia ixx="0.00506234058" ixy="0.00001431158" ixz="0.00015519082" iyy="0.0048013591" iyz="-0.00002707934" izz="0.00161030006"/>
</inertial>
</link>
<!-- SONARS -->
<!-- Nao has different sonar transmitter and receiver. The correct way is to take the intersection of the 2 cones -->
<!-- <joint name="RightSonarTransmitter" type="fixed">
<parent link="torso"/>
<child link="SonarRightTransmitter"/>
<origin xyz="0.0537 -0.0341 0.0698" rpy="0.0 -0.1745 -0.3490"/>
</joint>
<link name="SonarRightTransmitter"/>
<joint name="LeftSonarTransmitter" type="fixed">
<parent link="torso"/>
<child link="SonarLeftTransmitter"/>
<origin xyz="0.0537 0.0341 0.0698" rpy="0.0 -0.1745 0.3490"/>
</joint>
<link name="SonarLeftTransmitter"/> -->
<joint name="RightSonar" type="fixed">
<parent link="torso"/>
<child link="SonarRight"/>
<origin xyz="0.0477 -0.0416 0.0509" rpy="0 0.2618 -0.4363"/>
</joint>
<link name="SonarRight"/>
<joint name="LeftSonar" type="fixed">
<parent link="torso"/>
<child link="SonarLeft"/>
<origin xyz="0.0477 0.0416 0.0509" rpy="0 0.2618 0.4363"/>
</joint>
<link name="SonarLeft"/>
<!-- TORSO-HEAD JOINT -->
<joint name="HeadYaw" type="revolute">
<parent link="torso"/>
<child link="neck"/>
<limit effort="0.62" lower="-2.0857" upper="2.0857" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0.1265"/>
<axis xyz="0 0 1"/>
</joint>
<!-- HEAD -->
<link name="neck">
<xacro:visuals_collisions_head_yaw/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00001 0.00014 -0.02742"/>
<mass value="0.06442"/>
<inertia ixx="0.00007499295" ixy="0.00000000157" ixz="-0.00000001834" iyy="0.00007599995" iyz="-0.00000005295" izz="0.00000553373"/>
</inertial>
</link>
<joint name="HeadPitch" type="revolute">
<parent link="neck"/>
<child link="gaze"/>
<limit effort="0.62" lower="-0.6720" upper="0.5149" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="gaze">
<xacro:visuals_collisions_head_pitch/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00112 0.00003 0.05258"/>
<mass value="0.6533"/>
<inertia ixx="0.00263129518" ixy="0.00000878814" ixz="0.00004098466" iyy="0.00249112488" iyz="-0.00002995792" izz="0.00098573565"/>
</inertial>
</link>
<!-- CAMERAS -->
<joint name="TopCamera" type="fixed">
<parent link="gaze"/>
<child link="CameraTop"/>
<origin xyz="0.05871 0 0.06364" rpy="0 0.0209 0"/>
</joint>
<link name="CameraTop"/>
<joint name="BottomCamera" type="fixed">
<parent link="gaze"/>
<child link="CameraBottom"/>
<origin xyz="0.05071 0 0.01774" rpy="0 0.6929 0"/>
</joint>
<link name="CameraBottom"/>
<!-- ARM LINKS/JOINTS -->
<!-- Shoulder Roll Links are different for the 2 arms -->
<link name="LShoulderDummy">
<xacro:visuals_collisions_shoulder_roll side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
<mass value="0.15777"/>
<inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
</inertial>
</link>
<link name="RShoulderDummy">
<xacro:visuals_collisions_shoulder_roll side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
<mass value="0.15777"/>
<inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
</inertial>
</link>
<!-- XACRO -->
<xacro:macro name="arm_links_joints" params="side reflect">
<joint name="${side}ShoulderPitch" type="revolute">
<parent link="torso"/>
<child link="${side}Shoulder"/>
<limit effort="0.62" lower="-2.0857" upper="2.0857" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 ${reflect*0.098} 0.1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="${side}Shoulder">
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00165 ${reflect*(-0.02663)} 0.00014"/>
<mass value="0.07504"/>
<inertia ixx="0.00008428430" ixy="${reflect*-0.00000202802}" ixz="0.00000002338" iyy="0.00001415561" iyz="${reflect*-0.00000001972}" izz="0.00008641949"/>
</inertial>
</link>
<joint name="${side}ElbowYaw" type="revolute">
<parent link="${side}ShoulderDummy"/>
<child link="${side}Elbow"/>
<limit effort="0.62" lower="-2.0857" upper="2.0857" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0.105 ${reflect*0.015} 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="${side}Elbow">
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.02744 0 -0.00014"/>
<mass value="0.06483"/>
<inertia ixx="0.00000559715" ixy="0.00000000421" ixz="0.00000004319" iyy="0.00007543312" iyz="-0.00000000184" izz="0.00007644339"/>
</inertial>
</link>
<link name="${side}ElbowDummy">
<xacro:visuals_collisions_elbow_roll side="${side}"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02455 0.00563 0.0033"/>
<mass value="0.07761"/>
<inertia ixx="0.00009389993" ixy="-0.00004714452" ixz="-0.00002699471" iyy="0.00037151879" iyz="-0.00000245977" izz="0.00034190083"/>
</inertial>
</link>
</xacro:macro>
<xacro:arm_links_joints reflect="1" side="L"/>
<xacro:arm_links_joints reflect="-1" side="R"/>
<!-- JOINTS/LINKS THAT NEED TO BE NAMED FOR REP-120 -->
<joint name="LWristYaw" type="revolute">
<parent link="LElbowDummy"/>
<child link="l_wrist"/>
<origin rpy="0 0 0" xyz="0.05595 0 0"/>
<limit effort="0.49" lower="-1.8238" upper="1.8238" velocity="3.0"/>
</joint>
<link name="l_wrist">
<xacro:visuals_collisions_wrist_yaw reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
<mass value="0.18533"/>
<inertia ixx="0.00007054933" ixy="0.00000571599" ixz="-0.00002247437" iyy="0.00035606232" iyz="0.00000317771" izz="0.00035191933"/>
</inertial>
</link>
<!-- DUMMY REVOLUTE JOINT - JUST THERE TO RETURN VALUES BETWEEN 0.0 AND 1.0 THAT THE ROBOT RETURNS -->
<joint name="LHand" type="revolute">
<parent link="l_wrist"/>
<child link="l_gripper"/>
<origin rpy="0 0 0" xyz="0.05775 0 0"/>
<limit effort="0.49" lower="0.0" upper="1.0" velocity="3.0"/>
</joint>
<link name="l_gripper"/>
<joint name="RWristYaw" type="revolute">
<parent link="RElbowDummy"/>
<child link="r_wrist"/>
<origin rpy="0 0 0" xyz="0.05595 0 0"/>
<limit effort="0.49" lower="-1.8238" upper="1.8238" velocity="3.0"/>
</joint>
<link name="r_wrist">
<xacro:visuals_collisions_wrist_yaw reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.03434 -0.00088 0.00308"/>
<mass value="0.18533"/>
<inertia ixx="0.00007054933" ixy="0.00000571599" ixz="-0.00002247437" iyy="0.00035606232" iyz="0.00000317771" izz="0.00035191933"/>
</inertial>
</link>
<!-- DUMMY REVOLUTE JOINT - JUST THERE TO RETURN VALUES BETWEEN 0.0 AND 1.0 THAT THE ROBOT RETURNS -->
<joint name="RHand" type="revolute">
<parent link="r_wrist"/>
<child link="r_gripper"/>
<origin rpy="0 0 0" xyz="0.05775 0 0"/>
<limit effort="0.49" lower="0.0" upper="1.0" velocity="3.0"/>
</joint>
<link name="r_gripper"/>
<!-- JOINTS THAT ARE NOT IDENTICAL TO 2 ENDS -->
<joint name="LShoulderRoll" type="revolute">
<parent link="LShoulder"/>
<child link="LShoulderDummy"/>
<limit effort="0.62" lower="-0.3142" upper="1.3265" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="RShoulderRoll" type="revolute">
<parent link="RShoulder"/>
<child link="RShoulderDummy"/>
<limit effort="0.62" lower="-1.3265" upper="0.3142" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="LElbowRoll" type="revolute">
<parent link="LElbow"/>
<child link="LElbowDummy"/>
<limit effort="0.62" lower="-1.5446" upper="-0.0349" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<joint name="RElbowRoll" type="revolute">
<parent link="RElbow"/>
<child link="RElbowDummy"/>
<limit effort="0.62" lower="0.0349" upper="1.5446" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 0 1"/>
</joint>
<!-- END OF ARM LINKS/JOINTS -->
<!-- LEFT LEG LINKS/JOINTS -->
<joint name="LHipYawPitch" type="revolute">
<parent link="base_link"/>
<child link="LHipYaw"/>
<limit effort="1.61" lower="-1.145303" upper="0.740810" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0.05 -0.085"/>
<axis xyz="0 0.707106 -0.707106"/>
</joint>
<link name="LHipYaw">
<xacro:visuals_collisions_hip_yaw_pitch reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00781 -0.01114 0.02661"/>
<mass value="0.06981"/>
<inertia ixx="0.00008150233" ixy="-0.00000499449" ixz="0.00001274817" iyy="0.00010132555" iyz="0.00002345474" izz="0.000062626363"/>
</inertial>
</link>
<joint name="LHipRoll" type="revolute">
<parent link="LHipYaw"/>
<child link="LHip"/>
<limit effort="1.61" lower="-0.379472" upper="0.790477" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="LHip">
<xacro:visuals_collisions_hip_roll side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.01549 0.00029 -0.00515"/>
<mass value="0.13053"/>
<inertia ixx="0.00002758354" ixy="-0.00000002233" ixz="-0.00000408164" iyy="0.00009827055" iyz="-0.00000000419" izz="0.00008809973"/>
</inertial>
</link>
<joint name="LHipPitch" type="revolute">
<parent link="LHip"/>
<child link="LHipDummy"/>
<limit effort="1.61" lower="-1.535889" upper="0.484090" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="LHipDummy">
<xacro:visuals_collisions_hip_pitch reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00138 0.00221 -0.05373"/>
<mass value="0.38968"/>
<inertia ixx="0.00163671962" ixy="0.00000092451" ixz="0.00008530668" iyy="0.00159107278" iyz="0.00003836160" izz="0.00030374342"/>
</inertial>
</link>
<joint name="LKneePitch" type="revolute">
<parent link="LHipDummy"/>
<child link="LKnee"/>
<limit effort="1.61" lower="-0.092346" upper="2.112528" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="LKnee">
<xacro:visuals_collisions_knee_pitch reflect="1" side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00453 0.00225 -0.04936"/>
<mass value="0.29142"/>
<inertia ixx="0.00118207967" ixy="0.00000063362" ixz="0.00003649697" iyy="0.00112865226" iyz="0.00003949523" izz="0.00019322744"/>
</inertial>
</link>
<joint name="LAnklePitch" type="revolute">
<parent link="LKnee"/>
<child link="l_ankle"/>
<limit effort="1.61" lower="-1.189516" upper="0.922747" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1029"/>
<axis xyz="0 1 0"/>
</joint>
<link name="l_ankle">
<xacro:visuals_collisions_ankle_pitch side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00045 0.00029 0.00685"/>
<mass value="0.13416"/>
<inertia ixx="0.00003850978" ixy="-0.00000002634" ixz="0.00000386194" iyy="0.00007426526" iyz="0.00000001834" izz="0.00005486540"/>
</inertial>
</link>
<joint name="LAnkleRoll" type="revolute">
<parent link="l_ankle"/>
<child link="l_sole"/>
<limit effort="1.61" lower="-0.397880" upper="0.769001" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="l_sole">
<xacro:visuals_collisions_ankle_roll side="L"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
<mass value="0.16184"/>
<inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
</inertial>
</link>
<!-- RIGHT LEG LINKS/JOINTS -->
<joint name="RHipYawPitch" type="revolute">
<parent link="base_link"/>
<child link="RHipYaw"/>
<limit effort="1.61" lower="-1.145303" upper="0.740810" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 -0.05 -0.085"/>
<axis xyz="0 0.707106 0.707106"/>
<mimic joint="LHipYawPitch" multiplier="1" offset="0"/>
</joint>
<link name="RHipYaw">
<xacro:visuals_collisions_hip_yaw_pitch reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.00766 0.01200 0.02716"/>
<mass value="0.07118"/>
<inertia ixx="0.00008997195" ixy="0.00000500219" ixz="0.00001273525" iyy="0.00010552611" iyz="-0.00002770080" izz="0.00006688724"/>
</inertial>
</link>
<joint name="RHipRoll" type="revolute">
<parent link="RHipYaw"/>
<child link="RHip"/>
<limit effort="1.61" lower="-0.790477" upper="0.379472" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="RHip">
<xacro:visuals_collisions_hip_roll side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="-0.01549 -0.00029 -0.00516"/>
<mass value="0.13053"/>
<inertia ixx="0.00002758654" ixy="-0.00000001919" ixz="-0.00000410822" iyy="0.00009826996" iyz="0.00000000251" izz="0.00008810332"/>
</inertial>
</link>
<joint name="RHipPitch" type="revolute">
<parent link="RHip"/>
<child link="RHipDummy"/>
<limit effort="1.61" lower="-1.535889" upper="0.484090" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
<link name="RHipDummy">
<xacro:visuals_collisions_hip_pitch reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00139 -0.00225 -0.05374"/>
<mass value="0.38976"/>
<inertia ixx="0.00163748205" ixy="-0.00000083954" ixz="0.00008588301" iyy="0.00159221403" iyz="-0.00003917626" izz="0.00030397824"/>
</inertial>
</link>
<joint name="RKneePitch" type="revolute">
<parent link="RHipDummy"/>
<child link="RKnee"/>
<limit effort="1.61" lower="-0.103083" upper="2.120198" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1"/>
<axis xyz="0 1 0"/>
</joint>
<link name="RKnee">
<xacro:visuals_collisions_knee_pitch reflect="-1" side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00394 -0.00221 -0.04938"/>
<mass value="0.29163"/>
<inertia ixx="0.00118282956" ixy="-0.00000089650" ixz="0.00002799690" iyy="0.00112827851" iyz="-0.00003847604" izz="0.00019145277"/>
</inertial>
</link>
<joint name="RAnklePitch" type="revolute">
<parent link="RKnee"/>
<child link="r_ankle"/>
<limit effort="1.61" lower="-1.186448" upper="0.932056" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 -0.1029"/>
<axis xyz="0 1 0"/>
</joint>
<link name="r_ankle">
<xacro:visuals_collisions_ankle_pitch side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.00045 -0.00030 0.00684"/>
<mass value="0.13415"/>
<inertia ixx="0.00003850813" ixy="0.00000006434" ixz="0.00000387466" iyy="0.00007431082" iyz="-0.00000000458" izz="0.00005491312"/>
</inertial>
</link>
<joint name="RAnkleRoll" type="revolute">
<parent link="r_ankle"/>
<child link="r_sole"/>
<limit effort="1.61" lower="-0.768992" upper="0.397935" velocity="3.0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<axis xyz="1 0 0"/>
</joint>
<link name="r_sole">
<xacro:visuals_collisions_ankle_roll side="R"/>
<inertial>
<!-- CENTER OF MASS -->
<origin rpy="0 0 0" xyz="0.02540 -0.00332 -0.03239"/>
<mass value="0.16171"/>
<inertia ixx="0.00026930201" ixy="0.00000587505" ixz="0.00013913328" iyy="0.00064347385" iyz="-0.00001884917" izz="0.00052503477"/>
</inertial>
</link>
<!-- END OF LEG LINKS/JOINTS -->
</robot>

280
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_gazebo.xacro Normal file
View File

@ -0,0 +1,280 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- GAZEBO ROS CONTROL PLUGIN -->
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
<robotNamespace>/nao_dcm</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
</plugin>
</gazebo>
<!-- END OF GAZEBO ROS CONTROL PLUGIN -->
<!-- GAZEBO MATERIALS -->
<gazebo reference="torso">
<material>Gazebo/Blue</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="neck">
<material>Gazebo/Red</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="gaze">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LShoulderDummy">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RShoulderDummy">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LShoulder">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LElbow">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LElbowDummy">
<material>Gazebo/Red</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="l_wrist">
<material>Gazebo/Yellow</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="l_gripper">
<material>Gazebo/Yellow</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RShoulder">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RElbow">
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RElbowDummy">
<material>Gazebo/Red</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="r_wrist">
<material>Gazebo/Yellow</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="r_gripper">
<material>Gazebo/Yellow</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LHipYaw">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LHip">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LHipDummy">
<material>Gazebo/Blue</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="LKnee">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="l_ankle">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="l_sole">
<material>Gazebo/Blue</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
<fdir1>1 0 0</fdir1>
<maxVel>1.0</maxVel>
<minDepth>0.003</minDepth>
</gazebo>
<gazebo reference="RHipYaw">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RHip">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RHipDummy">
<material>Gazebo/Blue</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="RKnee">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="r_ankle">
<material>Gazebo/Green</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
</gazebo>
<gazebo reference="r_sole">
<material>Gazebo/Blue</material>
<mu1>0.5</mu1>
<mu2>0.5</mu2>
<selfCollide>false</selfCollide>
<turnGravityOff>false</turnGravityOff>
<fdir1>1 0 0</fdir1>
<maxVel>1.0</maxVel>
<minDepth>0.003</minDepth>
</gazebo>
<!-- END OF GAZEBO MATERIALS -->
<!-- Used for fixing robot to Gazebo 'base_link' -->
<!-- <link name="world"/><joint name="fixed" type="fixed"><parent link="world"/><child link="Torso"/><origin xyz="0 0 0.7" rpy="0 0 0" /></joint>-->
<!-- cameras -->
<gazebo reference="CameraTop">
<sensor type="camera" name="CameraTop">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>1.06290551</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.3</near>
<far>500</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>nao_robot/CameraTop</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>CameraTop</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
<gazebo reference="CameraBottom">
<sensor type="camera" name="CameraBottom">
<update_rate>30.0</update_rate>
<camera name="head">
<horizontal_fov>0.834267382</horizontal_fov>
<image>
<width>640</width>
<height>480</height>
<format>R8G8B8</format>
</image>
<clip>
<near>0.3</near>
<far>500</far>
</clip>
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.007</stddev>
</noise>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<alwaysOn>true</alwaysOn>
<updateRate>0.0</updateRate>
<cameraName>nao_robot/CameraBottom</cameraName>
<imageTopicName>image_raw</imageTopicName>
<cameraInfoTopicName>camera_info</cameraInfoTopicName>
<frameName>CameraBottom</frameName>
<hackBaseline>0.07</hackBaseline>
<distortionK1>0.0</distortionK1>
<distortionK2>0.0</distortionK2>
<distortionK3>0.0</distortionK3>
<distortionT1>0.0</distortionT1>
<distortionT2>0.0</distortionT2>
</plugin>
</sensor>
</gazebo>
</robot>

181
nao_dcm_common/nao_dcm_description/urdf/nao_dcm_visuals_collisions.xacro Normal file
View File

@ -0,0 +1,181 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- MATERIALS -->
<material name="Blue">
<color rgba="0 0 1 1"/>
</material>
<material name="Red">
<color rgba="1 0 0 1"/>
</material>
<material name="Green">
<color rgba="0 1 0 1"/>
</material>
<material name="Yellow">
<color rgba="1 1 0 1"/>
</material>
<!-- END OF MATERIALS -->
<!-- XACRO MACROS FOR VISUALS AND COLLISIONS -->
<xacro:macro name="visuals_collisions_torso" params="">
<visual>
<origin xyz="0 0 0.02075" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.1 0.2115"/>
</geometry>
<material name="Blue"/>
</visual>
<collision>
<origin xyz="0 0 0.02075" rpy="0 0 0"/>
<geometry>
<box size="0.04 0.1 0.2115"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_head_yaw" params="">
</xacro:macro>
<xacro:macro name="visuals_collisions_head_pitch" params="">
<visual>
<origin xyz="0 0 0.058" rpy="${PI_2} 0 0"/>
<geometry>
<cylinder radius="0.04" length="0.14"/>
</geometry>
<material name="Green"/>
</visual>
<collision>
<origin xyz="0 0 0.058" rpy="${PI_2} 0 0"/>
<geometry>
<cylinder radius="0.04" length="0.14"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_shoulder_roll" params="side">
<visual>
<origin xyz="0.045 0 0" rpy="${PI_2} 0 ${PI_2}"/>
<geometry>
<cylinder radius="0.02" length="0.09"/>
</geometry>
<material name="Green"/>
</visual>
<collision>
<origin xyz="0.045 0 0" rpy="${PI_2} 0 ${PI_2}"/>
<geometry>
<cylinder radius="0.02" length="0.09"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_elbow_roll" params="side">
<visual>
<origin xyz="0.025325 0 0" rpy="${PI_2} 0 ${PI_2}"/>
<geometry>
<cylinder radius="0.02" length="0.05065"/>
</geometry>
<material name="Red"/>
</visual>
<collision>
<origin xyz="0.025325 0 0" rpy="${PI_2} 0 ${PI_2}"/>
<geometry>
<cylinder radius="0.02" length="0.05065"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_wrist_yaw" params="side reflect">
<visual>
<origin xyz="0.029 0 0" rpy="${PI_2} 0 ${PI_2}"/>
<geometry>
<box size="0.03 0.02 0.058"/>
</geometry>
<material name="Yellow"/>
</visual>
<collision>
<origin xyz="0.029 0 0" rpy="${PI_2} 0 ${PI_2}"/>
<geometry>
<box size="0.03 0.02 0.058"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_hip_yaw_pitch" params="side reflect">
</xacro:macro>
<xacro:macro name="visuals_collisions_hip_roll" params="side">
</xacro:macro>
<xacro:macro name="visuals_collisions_hip_pitch" params="side reflect">
<visual>
<origin xyz="0 0 -0.05" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.1"/>
</geometry>
<material name="Yellow"/>
</visual>
<collision>
<origin xyz="0 0 -0.05" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.1"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_knee_pitch" params="side reflect">
<visual>
<origin xyz="0 0 -0.05" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.1"/>
</geometry>
<material name="Yellow"/>
</visual>
<collision>
<origin xyz="0 0 -0.05" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.1"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_ankle_pitch" params="side">
<visual>
<origin xyz="0 0 -0.023" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.046"/>
</geometry>
<material name="Yellow"/>
</visual>
<collision>
<origin xyz="0 0 -0.023" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.02" length="0.046"/>
</geometry>
</collision>
</xacro:macro>
<xacro:macro name="visuals_collisions_ankle_roll" params="side">
<visual>
<origin xyz="0.02 0 -0.042" rpy="0 0 0"/>
<geometry>
<box size="0.16 0.08 0.015"/>
</geometry>
<material name="Yellow"/>
</visual>
<collision>
<origin xyz="0.02 0 -0.042" rpy="0 0 0"/>
<geometry>
<box size="0.16 0.06 0.015"/>
</geometry>
</collision>
</xacro:macro>
<!-- END OF XACRO MACROS -->
</robot>

Some files were not shown because too many files have changed in this diff Show More