From 90965ccfdbcbb6d5b992dc2730b3df784d86a0b1 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Fri, 28 Mar 2014 21:15:32 +0900 Subject: [PATCH 01/38] added eusgazebo --- CMakeLists.txt | 162 +++++++++++++++++++++++++++++++++++++++++++++++++ package.xml | 57 +++++++++++++++++ 2 files changed, 219 insertions(+) create mode 100644 CMakeLists.txt create mode 100644 package.xml diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 00000000..c6e2bcbb --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,162 @@ +cmake_minimum_required(VERSION 2.8.3) +project(eusgazebo) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + gazebo_ros + roseus +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependencies might have been +## pulled in transitively but can be declared for certainty nonetheless: +## * add a build_depend tag for "message_generation" +## * add a run_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +# add_message_files( +# FILES +# Message1.msg +# Message2.msg +# ) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# std_msgs # Or other packages containing msgs +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if you package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES eusgazebo +# CATKIN_DEPENDS gazebo_ros roseus +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories( + ${catkin_INCLUDE_DIRS} +) + +## Declare a cpp library +# add_library(eusgazebo +# src/${PROJECT_NAME}/eusgazebo.cpp +# ) + +## Declare a cpp executable +# add_executable(eusgazebo_node src/eusgazebo_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(eusgazebo_node eusgazebo_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(eusgazebo_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS eusgazebo eusgazebo_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_eusgazebo.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/package.xml b/package.xml new file mode 100644 index 00000000..ef5b8562 --- /dev/null +++ b/package.xml @@ -0,0 +1,57 @@ + + + eusgazebo + 0.0.0 + The eusgazebo package + + + + + murooka + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + gazebo_ros + roseus + gazebo_ros + roseus + + + + + + + + + + + \ No newline at end of file From 7cc813ad90c02c4c894338c69e8f07c56d60fb25 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Fri, 28 Mar 2014 21:25:11 +0900 Subject: [PATCH 02/38] added eusgazebo/euslisp/eusgazebo-core.l --- euslisp/eusgazebo-core.l | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 euslisp/eusgazebo-core.l diff --git a/euslisp/eusgazebo-core.l b/euslisp/eusgazebo-core.l new file mode 100644 index 00000000..6c696536 --- /dev/null +++ b/euslisp/eusgazebo-core.l @@ -0,0 +1,2 @@ +;; eusgazebo-core.l + From c11a5cc49296bd4d3150766ca47f482d9f8a3a1c Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sat, 29 Mar 2014 23:14:32 +0900 Subject: [PATCH 03/38] implemtented core part of eusgazebo. you can execute test program by roseus eusgazebo.l "(test)" --- euslisp/eusgazebo-core.l | 2 - euslisp/eusgazebo-util.l | 99 ++++++++++++++++++ euslisp/eusgazebo.l | 210 +++++++++++++++++++++++++++++++++++++++ 3 files changed, 309 insertions(+), 2 deletions(-) delete mode 100644 euslisp/eusgazebo-core.l create mode 100644 euslisp/eusgazebo-util.l create mode 100644 euslisp/eusgazebo.l diff --git a/euslisp/eusgazebo-core.l b/euslisp/eusgazebo-core.l deleted file mode 100644 index 6c696536..00000000 --- a/euslisp/eusgazebo-core.l +++ /dev/null @@ -1,2 +0,0 @@ -;; eusgazebo-core.l - diff --git a/euslisp/eusgazebo-util.l b/euslisp/eusgazebo-util.l new file mode 100644 index 00000000..0398d164 --- /dev/null +++ b/euslisp/eusgazebo-util.l @@ -0,0 +1,99 @@ +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; urdf & gazebo util + +;; convert model +(defun irteus2urdf-for-gazebo + (model + &key + name + (collada-dir "/tmp") + (urdf-dir nil)) + + (cond + ((not (subclassp (class model) cascaded-link)) + (format t "invalid class, not childclass of cascaded-link~%")) + ((null (send model :links)) (format t "please setq links~%")) + (t + (setq name (send model :name)) + (if (or (not (stringp (send model :name))) + (zerop (length (send model :name)))) + (send model :name + (remove #\: (format nil "~A" (send model :name))))) + ;; convert to collada + (collada::eus2collada model collada-dir) + ;; convert to urdf + (cond + ((probe-file (format nil "~A/~A.dae" collada-dir (send model :name))) + (format t "generate ~A.dae~%convert to urdf~%" (send model :name)) + (unix:system + (format nil + "rosrun eusgazebo eus2urdf_for_gazebo.py ~a ~a/~a.dae ~a" + (send model :name) collada-dir (send model :name) (if urdf-dir urdf-dir "")))) + (t (format nil "dae error~%"))) + (format nil "~a/~a/~a" urdf-dir (send model :name name) "model.urdf") + ))) + + +;; spawn model +(defun spawn-model-to-gazebo + (urdf-model-path + &key + (model-name nil) + (model-coords (make-coords))) + + (let* ((model-pos (scale 0.001 (send model-coords :pos))) + (model-rpy (car (send model-coords :rpy-angle))) + (x (elt model-pos 0)) + (y (elt model-pos 1)) + (z (elt model-pos 2)) + (roll (elt model-rpy 2)) + (pitch (elt model-rpy 1)) + (yaw (elt model-rpy 0))) + (unix:system (format nil "gzfactory spawn -f ~a -x ~a -y ~a -z ~a -R ~a -P ~a -Y ~a ~a" + urdf-model-path x y z roll pitch yaw (if model-name (format nil "-m ~a" model-name) "")))) + ) + +;; delete model +(defun delete-model-to-gazebo + (model-name) + + (unix:system (format nil "gzfactory delete -m ~a" model-name)) + ) + + +;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; +;; ros data conversion util + +(defun pose->coords + (msg) + + (let* ((pos (scale 1000 (ros-xyz->eus-vec (send msg :position)))) + (quat (ros-xyzw->eus-vec (send msg :orientation))) + (rot (quaternion2matrix quat))) + (make-coords :pos pos :rot rot))) + +(defun ros-xyz->eus-vec + (msg) + + (float-vector (send msg :x) (send msg :y) (send msg :z))) + +(defun ros-xyzw->eus-vec + (msg) + + (float-vector (send msg :w) (send msg :x) (send msg :y) (send msg :z))) + +(defun coords->pose + (coords) + + (let* ((pose (instance geometry_msgs::Pose :init)) + (pos (scale 0.001 (send coords :worldpos))) + (rot (send coords :worldrot)) + (quat (matrix2quaternion rot))) + (send pose :position :x (elt pos 0)) + (send pose :position :y (elt pos 1)) + (send pose :position :z (elt pos 2)) + (send pose :orientation :x (elt quat 1)) + (send pose :orientation :y (elt quat 2)) + (send pose :orientation :z (elt quat 3)) + (send pose :orientation :w (elt quat 0)) + pose)) diff --git a/euslisp/eusgazebo.l b/euslisp/eusgazebo.l new file mode 100644 index 00000000..65261691 --- /dev/null +++ b/euslisp/eusgazebo.l @@ -0,0 +1,210 @@ +;; eusgazebo-core.l + +(load "package://eusgazebo/euslisp/eusgazebo-util.l") + +(ros::load-ros-manifest "gazebo_msgs") + +(ros::roseus "eusgazebo_client") + + +(defclass eusgazebo + :super propertied-object + :slots (model-list ground) + ) + +(defmethod eusgazebo + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; initialize + (:init + () + + ;; start gzserver + (warning-message 2 "launch gzserver~%") + (unix:system "mkdir /tmp/gazebo_model -p") + (unix:system "pkill gzserver") + (unix:system "GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/tmp/gazebo_model rosrun gazebo_ros gzserver &") + ;; subscribe gazebo state + (ros::subscribe "/gazebo/model_states" + gazebo_msgs::ModelStates #'send self :gazebo-model-state-cb) + ;; publish eus state + (ros::advertise "/gazebo/set_model_state" gazebo_msgs::ModelState 1) + ;; make ane view ground on eus viewer + (unless ground (setq ground (make-cube 5000 5000 1))) + (cond ((and (boundp '*irtviewer*) (not (member ground (send *irtviewer* :objects)))) + (objects ground)) + (t + (objects (list ground)))) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; add eus model to gazebo world + (:add-model + (model) + + ;; check if already added + (when (member model model-list) + (return-from :add-model nil)) + + ;; set model name + (send model :put :gazebo-model-name (send model :name)) ;; [ToDo] if the urdf model with same name is already exists, set other new name. + + ;; convert urdf + (let* (urdf-model-path) + (setq urdf-model-path (irteus2urdf-for-gazebo model :name (send model :get :gazebo-model-name) :urdf-dir "/tmp/gazebo_model")) + (send model :put :urdf-model-path urdf-model-path)) + + ;; spawn model + (spawn-model-to-gazebo (send model :get :urdf-model-path) :model-name (send model :get :gazebo-model-name) :model-coords (send model :copy-worldcoords)) + + ;; add to model-list + (setq model-list (append model-list (list model))) + (unix::usleep (* 100 1000)) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; delete model from gazebo world + (:delete-model + (model) + + ;; check if already added + (unless (member model model-list) + (return-from delete-model nil)) + + ;; delete model + (delete-model-to-gazebo (send model :get :gazebo-model-name)) + + ;; delete to model-list + (setq model-list (remove model model-list)) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; callback function of model_states topic + (:gazebo-model-state-cb + (msg) + + (dotimes (i (length (send msg :name))) + (let* ((name (elt (send msg :name) i)) + (pose (elt (send msg :pose) i)) + (twist (elt (send msg :twist) i)) + (model (find-if #'(lambda (_model) (string= (send _model :get :gazebo-model-name) name)) model-list))) + (when model + (send model :put :gazebo-pose (pose->coords pose))) + ))) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; update + (:update + () + (ros::spin-once) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; apply eus state to gazebo + (:eus2gzb + (&optional model) + + (cond (model + (let* ((msg (instance gazebo_msgs::ModelState :init))) + (send msg :model_name (send model :get :gazebo-model-name)) + (send msg :pose (coords->pose (send model :copy-worldcoords))) + (ros::publish "/gazebo/set_model_state" msg))) + (t ;; call for all drawed objects + (dolist (model model-list) (send self :eus2gzb model)))) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; apply gazebo state to eus + (:gzb2eus + (&optional model (update t)) + + (when update + (send self :update)) + (cond (model + (cond ((send model :get :gazebo-pose) + (send model :newcoords + (send model :get :gazebo-pose))) + (t + (warning-message 1 "failed to get gazebo pose~%")))) + (t ;; call for all drawed objects + (dolist (model model-list) (send self :gzb2eus model nil)))) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; apply gazebo state to eus in loop + (:gzb2eus-loop + (&optional model) + + (setq *gzb-loop-flag* t) + (do-until-key + (unless *gzb-loop-flag* + (return-from nil nil)) + (send self :gzb2eus model) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (unix::usleep (* 10 1000))) + (setq *gzb-loop-flag* nil)) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; apply gazebo state to eus in nonblock loop + (:gzb2eus-loop-nonblock + (&optional model) + + (warning-message 2 "call (break-gzb2eus-loop-nonblock) to break loop~%") + (unless (sys::free-threads) + (sys:make-thread 1)) + (sys:thread #'send self :gzb2eus-loop model) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; break from gzb2eus-loop-nonblock + (:break-gzb2eus-loop-nonblock + () + (setq *gzb-loop-flag* nil) + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; pause simulation + (:pause-sim + () + (call-empty-service "/gazebo/pause_physics") + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; unpause simulation + (:unpause-sim + () + (call-empty-service "/gazebo/unpause_physics") + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; reset world + (:reset-world + () + (call-empty-service "/gazebo/reset_world") + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; reset simulation + (:reset-sim + () + (call-empty-service "/gazebo/reset_sim") + ) + ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; + ;; reset simulation + (:model-list + () + model-list) + ) + + +(defun test + () + + ;; make and view eus model + (load "models/arrow-object.l") + (setq *arrow* (arrow)) + (objects (list *arrow*)) + + ;; generate eusgazebo instance + (setq *eg* (instance eusgazebo :init)) + (send *eg* :add-model *arrow*) + + ;; start simulation + (progn + ;; pause simulation + (send *eg* :pause-sim) + ;; set eus model pose + (send *arrow* :newcoords (make-coords :pos #f(0 0 1000) :rpy (list 0 1 0))) + ;; apply eus pose to gazebo + (send *eg* :eus2gzb) + ;; unpause simulation + (send *eg* :unpause-sim) + ;; view + (send *eg* :gzb2eus-loop))) From 55808906eb6281150c1d447a497a6e89e2500307 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sat, 29 Mar 2014 23:15:00 +0900 Subject: [PATCH 04/38] added jsk_model_tools to depand packages --- package.xml | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/package.xml b/package.xml index ef5b8562..444810a7 100644 --- a/package.xml +++ b/package.xml @@ -7,7 +7,7 @@ - murooka + murooka @@ -40,10 +40,9 @@ catkin - gazebo_ros - roseus gazebo_ros roseus + jsk_model_tools From 074fbb9132349b5301f8a90f20fd46d22805249d Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 10:59:54 +0900 Subject: [PATCH 05/38] added scripts/eus2urdf_for_gazebo.py --- scripts/eus2urdf_for_gazebo.py | 50 ++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100755 scripts/eus2urdf_for_gazebo.py diff --git a/scripts/eus2urdf_for_gazebo.py b/scripts/eus2urdf_for_gazebo.py new file mode 100755 index 00000000..e276693d --- /dev/null +++ b/scripts/eus2urdf_for_gazebo.py @@ -0,0 +1,50 @@ +#! /usr/bin/env python + +import sys +import os +import commands + + +def eus2urdf_for_gazebo (name, collada_path, urdf_path = (os.getenv("HOME") + "/.gazebo/models"), overwrite = True): + + urdf_dir_path = urdf_path + "/" + name + if overwrite: + os.system("rm -rf %s" % urdf_dir_path) + else: + print "[eus2urdf] check if the same name model already exits" + if os.path.exists(urdf_dir_path): + print '[ERROR eus2urdf] the same name model already exits' + exit(1) + + print "[eus2urdf] make directory for urdf" + os.mkdir(urdf_dir_path) + + add_line_string = '\file://%s\' % name + manifest_path = '%s/../manifest.xml' % urdf_dir_path + if len(commands.getoutput("grep %s %s" % (add_line_string, manifest_path))) == 0: + print "[eus2urdf] add file path to manifest.xml" + os.system('sed -i -e \"s@ @ %s\\n @g\" %s' % (add_line_string, manifest_path)) + + print "[eus2urdf] make model.config in urdf directory" + config_path = '%s/model.config' % urdf_dir_path + os.system('echo "\n\n %s\n 0.1.0\n model.urdf\n \n This model was automatically generated by converting the eus model.\n \n\n" > %s' % (name, config_path)) + + print "[eus2urdf] convert collada to urdf" + meshes_path = urdf_dir_path + '/meshes' + urdf_path = urdf_dir_path + '/' + 'model.urdf' + os.mkdir(meshes_path) + os.system('rosrun collada_tools collada_to_urdf %s -G -A --mesh_output_dir %s --mesh_prefix "model://%s/meshes" -O %s' % (collada_path, meshes_path, name, urdf_path)) + os.system('sed -i -e "s@continuous@revolute@g" %s' % urdf_path) + os.system('sed -i -e \"s@\\n false<\/static>\\n <\/gazebo>\\n @@g\" %s' % urdf_path) + # os.system('sed -i -e \"s@ @ @g\" %s' % urdf_path) + + +if __name__ == '__main__': + if len(sys.argv) > 3: + eus2urdf_for_gazebo(sys.argv[1], sys.argv[2], sys.argv[3]) + elif len(sys.argv) > 2: + eus2urdf_for_gazebo(sys.argv[1], sys.argv[2]) From 9feb9abc4e66c7bb618b0b9e955f362519ac2b45 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 11:34:06 +0900 Subject: [PATCH 06/38] add author, remove comments --- package.xml | 46 ++++------------------------------------------ 1 file changed, 4 insertions(+), 42 deletions(-) diff --git a/package.xml b/package.xml index 444810a7..fc302709 100644 --- a/package.xml +++ b/package.xml @@ -1,56 +1,18 @@ - eusgazebo 0.0.0 The eusgazebo package + Masaki Murooka - - - - murooka + BSD - - - - - TODO - - - - - + Masaki Murooka - - - - - - - - - - - - - - - - - catkin + gazebo_ros roseus jsk_model_tools - - - - - - - - - - \ No newline at end of file From 3a890ffab3f7a7f7b8481e7964b9411df1cf8fb1 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 11:54:45 +0900 Subject: [PATCH 07/38] added manifest.xml --- manifest.xml | 15 +++++++++++++++ package.xml | 3 ++- 2 files changed, 17 insertions(+), 1 deletion(-) create mode 100644 manifest.xml diff --git a/manifest.xml b/manifest.xml new file mode 100644 index 00000000..3ac18369 --- /dev/null +++ b/manifest.xml @@ -0,0 +1,15 @@ + + + + The eusgazebo package + + + Masaki Murooka (murooka@jsk.t.u-tokyo.ac.jp) + BSD + + + + + + + diff --git a/package.xml b/package.xml index fc302709..d0b90690 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,8 @@ eusgazebo - 0.0.0 + 0.1.0 The eusgazebo package + Kei Okada Masaki Murooka BSD From 64b069c301b0a56b4e60710d9cc404ef2a1da481 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 12:29:32 +0900 Subject: [PATCH 08/38] added Makefile, catkin.cake, remove comment lines, add install and test --- CMakeLists.txt | 168 +++---------------------------------------------- Makefile | 2 + catkin.cmake | 14 +++++ 3 files changed, 24 insertions(+), 160 deletions(-) create mode 100644 Makefile create mode 100644 catkin.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index c6e2bcbb..bd486c55 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,162 +1,10 @@ -cmake_minimum_required(VERSION 2.8.3) -project(eusgazebo) +if(NOT USE_ROSBUILD) + include(catkin.cmake) + return() +endif() +cmake_minimum_required(VERSION 2.4.6) +include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) -## Find catkin macros and libraries -## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) -## is used, also find other catkin packages -find_package(catkin REQUIRED COMPONENTS - gazebo_ros - roseus -) +rosbuild_init() -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependencies might have been -## pulled in transitively but can be declared for certainty nonetheless: -## * add a build_depend tag for "message_generation" -## * add a run_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -# add_message_files( -# FILES -# Message1.msg -# Message2.msg -# ) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) - -## Generate added messages and services with any dependencies listed here -# generate_messages( -# DEPENDENCIES -# std_msgs # Or other packages containing msgs -# ) - -################################### -## catkin specific configuration ## -################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if you package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( -# INCLUDE_DIRS include -# LIBRARIES eusgazebo -# CATKIN_DEPENDS gazebo_ros roseus -# DEPENDS system_lib -) - -########### -## Build ## -########### - -## Specify additional locations of header files -## Your package locations should be listed before other locations -# include_directories(include) -include_directories( - ${catkin_INCLUDE_DIRS} -) - -## Declare a cpp library -# add_library(eusgazebo -# src/${PROJECT_NAME}/eusgazebo.cpp -# ) - -## Declare a cpp executable -# add_executable(eusgazebo_node src/eusgazebo_node.cpp) - -## Add cmake target dependencies of the executable/library -## as an example, message headers may need to be generated before nodes -# add_dependencies(eusgazebo_node eusgazebo_generate_messages_cpp) - -## Specify libraries to link a library or executable target against -# target_link_libraries(eusgazebo_node -# ${catkin_LIBRARIES} -# ) - -############# -## Install ## -############# - -# all install targets should use catkin DESTINATION variables -# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html - -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS eusgazebo eusgazebo_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) - -############# -## Testing ## -############# - -## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test test/test_eusgazebo.cpp) -# if(TARGET ${PROJECT_NAME}-test) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) -# endif() - -## Add folders to be run by python nosetests -# catkin_add_nosetests(test) +#rosbuild_add_rostest(test/fall-arrow-object-simulation.test) diff --git a/Makefile b/Makefile new file mode 100644 index 00000000..73e8a917 --- /dev/null +++ b/Makefile @@ -0,0 +1,2 @@ +EXTRA_CMAKE_FLAGS = -DUSE_ROSBUILD:BOOL=1 +include $(shell rospack find mk)/cmake.mk diff --git a/catkin.cmake b/catkin.cmake new file mode 100644 index 00000000..9f8db4f0 --- /dev/null +++ b/catkin.cmake @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 2.8.3) +project(eusgazebo) + +catkin_package() + +find_package(catkin REQUIRED COMPONENTS rostest) + +## Install ## +install(DIRECTORY euslisp test scripts + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +## Testing ## +#add_rostest(test/fall-arrow-object-simulation.test) From 15713129ab98390da17e56b615c4922f48a6d6b8 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 12:49:23 +0900 Subject: [PATCH 09/38] added sample and test files --- CMakeLists.txt | 2 +- catkin.cmake | 4 +-- euslisp/eusgazebo.l | 25 ---------------- samples/fall-arrow-object-simulation.l | 33 +++++++++++++++++++++ test/test-fall-arrow-object-simulation.l | 13 ++++++++ test/test-fall-arrow-object-simulation.test | 5 ++++ 6 files changed, 54 insertions(+), 28 deletions(-) create mode 100644 samples/fall-arrow-object-simulation.l create mode 100755 test/test-fall-arrow-object-simulation.l create mode 100644 test/test-fall-arrow-object-simulation.test diff --git a/CMakeLists.txt b/CMakeLists.txt index bd486c55..d3025e26 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,4 +7,4 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) rosbuild_init() -#rosbuild_add_rostest(test/fall-arrow-object-simulation.test) +#rosbuild_add_rostest(test/test-fall-arrow-object-simulation.test) diff --git a/catkin.cmake b/catkin.cmake index 9f8db4f0..6e3e680a 100644 --- a/catkin.cmake +++ b/catkin.cmake @@ -6,9 +6,9 @@ catkin_package() find_package(catkin REQUIRED COMPONENTS rostest) ## Install ## -install(DIRECTORY euslisp test scripts +install(DIRECTORY euslisp test scripts samples DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS) ## Testing ## -#add_rostest(test/fall-arrow-object-simulation.test) +#add_rostest(test/test-fall-arrow-object-simulation.test) diff --git a/euslisp/eusgazebo.l b/euslisp/eusgazebo.l index 65261691..edd89387 100644 --- a/euslisp/eusgazebo.l +++ b/euslisp/eusgazebo.l @@ -183,28 +183,3 @@ model-list) ) - -(defun test - () - - ;; make and view eus model - (load "models/arrow-object.l") - (setq *arrow* (arrow)) - (objects (list *arrow*)) - - ;; generate eusgazebo instance - (setq *eg* (instance eusgazebo :init)) - (send *eg* :add-model *arrow*) - - ;; start simulation - (progn - ;; pause simulation - (send *eg* :pause-sim) - ;; set eus model pose - (send *arrow* :newcoords (make-coords :pos #f(0 0 1000) :rpy (list 0 1 0))) - ;; apply eus pose to gazebo - (send *eg* :eus2gzb) - ;; unpause simulation - (send *eg* :unpause-sim) - ;; view - (send *eg* :gzb2eus-loop))) diff --git a/samples/fall-arrow-object-simulation.l b/samples/fall-arrow-object-simulation.l new file mode 100644 index 00000000..f808c0d1 --- /dev/null +++ b/samples/fall-arrow-object-simulation.l @@ -0,0 +1,33 @@ +(load "package://eusgazebo/euslisp/eusgazebo.l") + + +(defun fall-arrow-object-simulation + () + + ;; make and view eus model + (load "models/arrow-object.l") + (setq *arrow* (arrow)) + (objects (list *arrow*)) + + ;; generate eusgazebo instance + (setq *eusgazebo-server* (instance eusgazebo :init)) + (send *eusgazebo-server* :add-model *arrow*) + + ;; start simulation + (progn + ;; pause simulation + (send *eusgazebo-server* :pause-sim) + ;; set eus model pose + (send *arrow* :newcoords (make-coords :pos #f(0 0 1000) :rpy (list 0 1 0))) + ;; apply eus pose to gazebo + (send *eusgazebo-server* :eus2gzb) + ;; unpause simulation + (send *eusgazebo-server* :unpause-sim) + ;; view + (dotimes (i 500) + (send *eusgazebo-server* :gzb2eus) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (unix::usleep (* 10 1000))) + ;;(send *eusgazebo-server* :gzb2eus-loop) + )) diff --git a/test/test-fall-arrow-object-simulation.l b/test/test-fall-arrow-object-simulation.l new file mode 100755 index 00000000..b3630421 --- /dev/null +++ b/test/test-fall-arrow-object-simulation.l @@ -0,0 +1,13 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") + +(load "package://eusgazebo/euslisp/samples/fall-arrow-object-simulation.l") + +(init-unit-test) + +(deftest test-fall-arrow-object-simulation + (fall-arrow-object-simulation)) + +(run-all-tests) +(exit) diff --git a/test/test-fall-arrow-object-simulation.test b/test/test-fall-arrow-object-simulation.test new file mode 100644 index 00000000..184e2fdc --- /dev/null +++ b/test/test-fall-arrow-object-simulation.test @@ -0,0 +1,5 @@ + + + + From e30aba44446f60cfb1e8b190b82d3a6853fd0d23 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 14:31:40 +0900 Subject: [PATCH 10/38] fixed test --- manifest.xml | 2 +- package.xml | 2 +- test/test-fall-arrow-object-simulation.l | 2 +- test/test-fall-arrow-object-simulation.test | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/manifest.xml b/manifest.xml index 3ac18369..fe8797d3 100644 --- a/manifest.xml +++ b/manifest.xml @@ -11,5 +11,5 @@ - + diff --git a/package.xml b/package.xml index d0b90690..e641eff8 100644 --- a/package.xml +++ b/package.xml @@ -15,5 +15,5 @@ gazebo_ros roseus - jsk_model_tools + collada_tools \ No newline at end of file diff --git a/test/test-fall-arrow-object-simulation.l b/test/test-fall-arrow-object-simulation.l index b3630421..e70a4696 100755 --- a/test/test-fall-arrow-object-simulation.l +++ b/test/test-fall-arrow-object-simulation.l @@ -2,7 +2,7 @@ (require :unittest "lib/llib/unittest.l") -(load "package://eusgazebo/euslisp/samples/fall-arrow-object-simulation.l") +(load "package://eusgazebo/samples/fall-arrow-object-simulation.l") (init-unit-test) diff --git a/test/test-fall-arrow-object-simulation.test b/test/test-fall-arrow-object-simulation.test index 184e2fdc..ed628c81 100644 --- a/test/test-fall-arrow-object-simulation.test +++ b/test/test-fall-arrow-object-simulation.test @@ -1,5 +1,5 @@ - From cd48d462ca05d475c79fe3fd5b1d5bb5869d9a17 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 14:54:29 +0900 Subject: [PATCH 11/38] added rosdep for gazebo_ros to manifest.xml --- manifest.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/manifest.xml b/manifest.xml index fe8797d3..3f2457e4 100644 --- a/manifest.xml +++ b/manifest.xml @@ -9,7 +9,10 @@ + + + From 126a36531d38c5d7f89fec5bc6162e44bd80e81a Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 15:24:46 +0900 Subject: [PATCH 12/38] fixed test/test-fall-arrow-object-simulation.test --- test/test-fall-arrow-object-simulation.test | 1 - 1 file changed, 1 deletion(-) diff --git a/test/test-fall-arrow-object-simulation.test b/test/test-fall-arrow-object-simulation.test index ed628c81..3c4100c1 100644 --- a/test/test-fall-arrow-object-simulation.test +++ b/test/test-fall-arrow-object-simulation.test @@ -1,5 +1,4 @@ - From df0b064f7e1d2bf54b7870881f29fa0e5e78597a Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 16:27:59 +0900 Subject: [PATCH 13/38] enable test in Makefile --- CMakeLists.txt | 2 +- catkin.cmake | 2 +- samples/fall-arrow-object-simulation.l | 1 + test/test-fall-arrow-object-simulation.l | 3 ++- 4 files changed, 5 insertions(+), 3 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index d3025e26..a672f684 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,4 +7,4 @@ include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) rosbuild_init() -#rosbuild_add_rostest(test/test-fall-arrow-object-simulation.test) +rosbuild_add_rostest(test/test-fall-arrow-object-simulation.test) diff --git a/catkin.cmake b/catkin.cmake index 6e3e680a..dbd5ab1f 100644 --- a/catkin.cmake +++ b/catkin.cmake @@ -11,4 +11,4 @@ install(DIRECTORY euslisp test scripts samples USE_SOURCE_PERMISSIONS) ## Testing ## -#add_rostest(test/test-fall-arrow-object-simulation.test) +add_rostest(test/test-fall-arrow-object-simulation.test) diff --git a/samples/fall-arrow-object-simulation.l b/samples/fall-arrow-object-simulation.l index f808c0d1..20000539 100644 --- a/samples/fall-arrow-object-simulation.l +++ b/samples/fall-arrow-object-simulation.l @@ -12,6 +12,7 @@ ;; generate eusgazebo instance (setq *eusgazebo-server* (instance eusgazebo :init)) (send *eusgazebo-server* :add-model *arrow*) + (unix::usleep (* 1000 1000)) ;; start simulation (progn diff --git a/test/test-fall-arrow-object-simulation.l b/test/test-fall-arrow-object-simulation.l index e70a4696..ff097e1d 100755 --- a/test/test-fall-arrow-object-simulation.l +++ b/test/test-fall-arrow-object-simulation.l @@ -7,7 +7,8 @@ (init-unit-test) (deftest test-fall-arrow-object-simulation - (fall-arrow-object-simulation)) + (fall-arrow-object-simulation) + (unix:system "pkill gzserver")) (run-all-tests) (exit) From 660a0172757ca7a671477f89a36fbb4f48f1999f Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 30 Mar 2014 19:47:16 +0900 Subject: [PATCH 14/38] added build dependencies in package.xml --- manifest.xml | 2 -- package.xml | 9 +++++++++ 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/manifest.xml b/manifest.xml index 3f2457e4..0555edf8 100644 --- a/manifest.xml +++ b/manifest.xml @@ -9,8 +9,6 @@ - - diff --git a/package.xml b/package.xml index e641eff8..a28e96fc 100644 --- a/package.xml +++ b/package.xml @@ -9,11 +9,20 @@ + Kei Okada Masaki Murooka catkin + gazebo_ros + roseus + collada_tools + gazebo_ros roseus collada_tools + + + euslisp + \ No newline at end of file From a30636770eb327b820f71a2ea6246bb7bde74579 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 6 Apr 2014 21:39:47 +0900 Subject: [PATCH 15/38] deleted incorrect line in package.xml --- package.xml | 3 --- 1 file changed, 3 deletions(-) diff --git a/package.xml b/package.xml index a28e96fc..9975688f 100644 --- a/package.xml +++ b/package.xml @@ -22,7 +22,4 @@ roseus collada_tools - - euslisp - \ No newline at end of file From 1295285bf2b3a04b0f37761c8fbdaae7e3026a22 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 7 Apr 2014 12:02:58 +0900 Subject: [PATCH 16/38] added mass-property-util.l --- euslisp/mass-property-util.l | 90 ++++++++++++++++++++++++++++++++++++ 1 file changed, 90 insertions(+) create mode 100644 euslisp/mass-property-util.l diff --git a/euslisp/mass-property-util.l b/euslisp/mass-property-util.l new file mode 100644 index 00000000..ebb20308 --- /dev/null +++ b/euslisp/mass-property-util.l @@ -0,0 +1,90 @@ +;; overwrite mass property ;;;;;;;;;;;;;;; +(defun overwirte-mass-property + (obj link-mass link-inertia-diag-elem) + + (dolist (link (send obj :links)) + (overwrite-link-mass link link-mass) + (overwrite-link-inertia link link-inertia-diag-elem)) + ) + +(defun overwirte-mass-property-if-too-small + (obj) + + (let* ((link-weight-min 1e6) ;; [g] + (link-inertia-diag-min (list 1e6 1e6 1e6)) ;; [gmm^2] + ) + (dolist (link (send obj :links)) + (let* ((link-weight (slot link bodyset-link 'weight)) + (link-inertia (slot (car (send obj :links)) bodyset-link 'inertia-tensor)) + (link-inertia-diag (mapcar #'(lambda (i) (elt (matrix-row link-inertia i) i)) (list 0 1 2)))) + ;; overwirte mass if too small + (when (< link-weight link-weight-min) + (warning-message 1 "overwrite mass because original value is too small: ~a [g] -> ~a [g]~%" link-weight link-weight-min) + (overwrite-link-mass link link-weight-min)) + ;; overwrite inertia if too small + (let* ((link-inertia-diag-overwirte + (mapcar #'max link-inertia-diag link-inertia-diag-min))) + (unless (equal link-inertia-diag link-inertia-diag-overwirte) + (warning-message 1 "overwrite inertia because original value is too small: ~a [gmm^2] -> ~a [gmm^2]~%" + link-inertia-diag link-inertia-diag-overwirte) + (overwrite-link-inertia link link-inertia-diag-overwirte))) + )))) + +(defun overwrite-link-mass + (link link-mass) + + (setf (slot link bodyset-link 'weight) link-mass)) + +(defun overwrite-link-inertia + (link link-inertia-diag-elem) ;; link-inertia-diag-elem shoud be 3-length list + ;; this function handles only diag element + + (dolist (idx (list 0 1 2)) + (let* ((link-inertia (slot link bodyset-link 'inertia-tensor)) + (tmp-row (matrix-row link-inertia idx))) + (setf (elt tmp-row idx) (elt link-inertia-diag-elem idx)) + (setf (matrix-row link-inertia idx) tmp-row)))) + + +;; get mass property ;;;;;;;;;;;;;;; +(defun get-link-mass + (link) + + (slot link bodyset-link 'weight)) + +(defun get-link-inertia + (link) + + (slot link bodyset-link 'inertia-tensor)) + +(defun print-obj-mass-property + (obj) + + (warning-message 2 "whole mass: ~a [g]~%first link inertia: ~a [gmm^2]~%" + (send obj :weight) (get-link-inertia (car (send obj :links))))) + + +;; caluculate valid mass property automatically ;;;;;;;;;;;;;;; +(defun get-obj-bounding-box-min-max-point + (obj) + + (let* ((min-point (list *inf* *inf* *inf*)) + (min-point-list + (mapcar #'(lambda (face) (send (send face :box) :minpoint)) + (send obj :faces))) + (max-point (list *-inf* *-inf* *-inf*)) + (max-point-list + (mapcar #'(lambda (face) (send (send face :box) :maxpoint)) + (send obj :faces)))) + (dolist (p min-point-list) + (dolist (idx (list 0 1 2)) + (when (< (elt p idx) (elt min-point idx)) + (setf (elt min-point idx) (elt p idx))) + )) + + (dolist (p max-point-list) + (dolist (idx (list 0 1 2)) + (when (> (elt p idx) (elt max-point idx)) + (setf (elt max-point idx) (elt p idx))) + )) + (list min-point max-point))) From 7257eaa1989424bbd0930e93988ba169ccecc1a0 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 7 Apr 2014 14:49:24 +0900 Subject: [PATCH 17/38] added domino-cube-object.l --- samples/domino-cube-object.l | 60 ++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 samples/domino-cube-object.l diff --git a/samples/domino-cube-object.l b/samples/domino-cube-object.l new file mode 100644 index 00000000..d3f331c9 --- /dev/null +++ b/samples/domino-cube-object.l @@ -0,0 +1,60 @@ +;; +;; DO NOT EDIT THIS FILE +;; this file is automatically generated from euslisp+euslib version +;; +;; +(defclass domino-cube-object + :super cascaded-link + :slots (sensors + )) +(defmethod domino-cube-object + (:init + (&rest args &key (name "domino-cube") (pos (float-vector 0 0 0)) (rot (unit-matrix 3)) &allow-other-keys) + (let (c bc + blink0 + ) + (send-super* :init :name name args) + + ;; definition of link + + ;; definition of :domino-cube-bodyset2 + (setq bc (list + (instance faceset :init :faces (list + (instance face :init :vertices (list (float-vector 150.0 500.0 2000.0) (float-vector -150.0 500.0 2000.0) (float-vector -150.0 -500.0 2000.0) (float-vector 150.0 -500.0 2000.0))) + (instance face :init :vertices (list (float-vector -150.0 500.0 0.0) (float-vector 150.0 500.0 0.0) (float-vector 150.0 -500.0 0.0) (float-vector -150.0 -500.0 0.0))) + (instance face :init :vertices (list (float-vector 150.0 500.0 2000.0) (float-vector 150.0 500.0 0.0) (float-vector -150.0 500.0 0.0) (float-vector -150.0 500.0 2000.0))) + (instance face :init :vertices (list (float-vector 150.0 -500.0 2000.0) (float-vector 150.0 -500.0 0.0) (float-vector 150.0 500.0 0.0) (float-vector 150.0 500.0 2000.0))) + (instance face :init :vertices (list (float-vector -150.0 -500.0 2000.0) (float-vector -150.0 -500.0 0.0) (float-vector 150.0 -500.0 0.0) (float-vector 150.0 -500.0 2000.0))) + (instance face :init :vertices (list (float-vector -150.0 500.0 2000.0) (float-vector -150.0 500.0 0.0) (float-vector -150.0 -500.0 0.0) (float-vector -150.0 -500.0 2000.0))) + )) + )) + (dolist (b (cdr bc)) (send (car bc) :assoc b)) + (send (elt bc 0) :set-color :gray40) + (setq blink0 (instance bodyset-link :init (make-cascoords) :bodies bc :name :domino-cube-bodyset2 :weight 2000 :centroid (float-vector 0.0 0.0 0.0) :inertia-tensor #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) + + ;; definition of assoc + (send self :assoc blink0) + + ;; definition of end-coords + + ;; definition of joint + + + ;; init-ending + (setq links (list blink0)) + (setq joint-list (list)) + (send self :init-ending) + (send self :move-to (make-coords :pos pos :rot rot)) + (send-all links :worldcoords) + + self)) + (:cameras (&rest args) + (forward-message-to-all (list) args)) + + (:handle (&rest args) (forward-message-to-all (list ) args)) + (:attention (&rest args) (forward-message-to-all (list ) args)) + (:button (&rest args) (forward-message-to-all (list ) args)) + ) + +(defun domino-cube (&rest args) (instance* domino-cube-object :init args)) +;; (format *error-output* "(instance domino-cube-object :init) for generating model~%") From 5a1d223d24e295de2a386673cc7be66d1fde5d87 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 7 Apr 2014 14:53:30 +0900 Subject: [PATCH 18/38] overwrite mass property with proper value and deal with the eus models with same name. --- euslisp/eusgazebo-util.l | 28 ++++++++++++---------------- euslisp/eusgazebo.l | 27 +++++++++++++++++++++++---- euslisp/mass-property-util.l | 7 ++++--- scripts/eus2urdf_for_gazebo.py | 11 +++++------ 4 files changed, 44 insertions(+), 29 deletions(-) diff --git a/euslisp/eusgazebo-util.l b/euslisp/eusgazebo-util.l index 0398d164..bef10ba1 100644 --- a/euslisp/eusgazebo-util.l +++ b/euslisp/eusgazebo-util.l @@ -5,7 +5,7 @@ (defun irteus2urdf-for-gazebo (model &key - name + (name (send model :name)) (collada-dir "/tmp") (urdf-dir nil)) @@ -14,23 +14,19 @@ (format t "invalid class, not childclass of cascaded-link~%")) ((null (send model :links)) (format t "please setq links~%")) (t - (setq name (send model :name)) - (if (or (not (stringp (send model :name))) - (zerop (length (send model :name)))) - (send model :name - (remove #\: (format nil "~A" (send model :name))))) ;; convert to collada (collada::eus2collada model collada-dir) - ;; convert to urdf - (cond - ((probe-file (format nil "~A/~A.dae" collada-dir (send model :name))) - (format t "generate ~A.dae~%convert to urdf~%" (send model :name)) - (unix:system - (format nil - "rosrun eusgazebo eus2urdf_for_gazebo.py ~a ~a/~a.dae ~a" - (send model :name) collada-dir (send model :name) (if urdf-dir urdf-dir "")))) - (t (format nil "dae error~%"))) - (format nil "~a/~a/~a" urdf-dir (send model :name name) "model.urdf") + ;; convert to urdf + (let* ((collada-path (format nil "~a/~a.dae" collada-dir (send model :name)))) + (cond + ((probe-file collada-path) + (format t "generate ~A.dae~%convert to urdf~%" name) + (unix:system + (format nil + "rosrun eusgazebo eus2urdf_for_gazebo.py ~a ~a ~a" + name collada-path (if urdf-dir urdf-dir "")))) + (t (format nil "dae error~%")))) + (format nil "~a/~a/~a" urdf-dir name "model.urdf") ))) diff --git a/euslisp/eusgazebo.l b/euslisp/eusgazebo.l index edd89387..c6c03bf6 100644 --- a/euslisp/eusgazebo.l +++ b/euslisp/eusgazebo.l @@ -1,6 +1,7 @@ ;; eusgazebo-core.l (load "package://eusgazebo/euslisp/eusgazebo-util.l") +(load "package://eusgazebo/euslisp/mass-property-util.l") (ros::load-ros-manifest "gazebo_msgs") @@ -38,18 +39,36 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;; add eus model to gazebo world (:add-model - (model) + (model + &key (urdf-dir "/tmp/gazebo_model")) ;; check if already added (when (member model model-list) (return-from :add-model nil)) - ;; set model name - (send model :put :gazebo-model-name (send model :name)) ;; [ToDo] if the urdf model with same name is already exists, set other new name. + ;; check mass property + (overwirte-mass-property-if-too-small model) + + ;; overwrite model name + (cond + ;; deal with the objects with no name (objects with no name cause an error in converting) + ((null (send model :name)) (send model :name "no-name")) + ;; deal with the objects with symbol name + ((symbolp (send model :name)) (send model :name (string-downcase (send model :name))))) + + ;; set gazebo model name + (send model :put :gazebo-model-name (send model :name)) + (let* ((obj-cnt 0)) + (while t + (let* ((model-file-path + (format nil "~a/~a" urdf-dir (send model :get :gazebo-model-name)))) + (unless (probe-file model-file-path) (return))) + (send model :put :gazebo-model-name (format nil "~a-~a" (send model :name) obj-cnt)) + (incf obj-cnt))) ;; convert urdf (let* (urdf-model-path) - (setq urdf-model-path (irteus2urdf-for-gazebo model :name (send model :get :gazebo-model-name) :urdf-dir "/tmp/gazebo_model")) + (setq urdf-model-path (irteus2urdf-for-gazebo model :name (send model :get :gazebo-model-name) :urdf-dir urdf-dir)) (send model :put :urdf-model-path urdf-model-path)) ;; spawn model diff --git a/euslisp/mass-property-util.l b/euslisp/mass-property-util.l index ebb20308..eb6696b1 100644 --- a/euslisp/mass-property-util.l +++ b/euslisp/mass-property-util.l @@ -10,7 +10,7 @@ (defun overwirte-mass-property-if-too-small (obj) - (let* ((link-weight-min 1e6) ;; [g] + (let* ((link-weight-min 1) ;; [g] (link-inertia-diag-min (list 1e6 1e6 1e6)) ;; [gmm^2] ) (dolist (link (send obj :links)) @@ -19,13 +19,13 @@ (link-inertia-diag (mapcar #'(lambda (i) (elt (matrix-row link-inertia i) i)) (list 0 1 2)))) ;; overwirte mass if too small (when (< link-weight link-weight-min) - (warning-message 1 "overwrite mass because original value is too small: ~a [g] -> ~a [g]~%" link-weight link-weight-min) + (warning-message 3 "overwrite mass because original value is too small: ~a [g] -> ~a [g]~%" link-weight link-weight-min) (overwrite-link-mass link link-weight-min)) ;; overwrite inertia if too small (let* ((link-inertia-diag-overwirte (mapcar #'max link-inertia-diag link-inertia-diag-min))) (unless (equal link-inertia-diag link-inertia-diag-overwirte) - (warning-message 1 "overwrite inertia because original value is too small: ~a [gmm^2] -> ~a [gmm^2]~%" + (warning-message 3 "overwrite inertia because original value is too small: ~a [gmm^2] -> ~a [gmm^2]~%" link-inertia-diag link-inertia-diag-overwirte) (overwrite-link-inertia link link-inertia-diag-overwirte))) )))) @@ -65,6 +65,7 @@ ;; caluculate valid mass property automatically ;;;;;;;;;;;;;;; +;; [ToDo] estimate mass and inertia from shape and density (defun get-obj-bounding-box-min-max-point (obj) diff --git a/scripts/eus2urdf_for_gazebo.py b/scripts/eus2urdf_for_gazebo.py index e276693d..e8b07946 100755 --- a/scripts/eus2urdf_for_gazebo.py +++ b/scripts/eus2urdf_for_gazebo.py @@ -20,10 +20,10 @@ def eus2urdf_for_gazebo (name, collada_path, urdf_path = (os.getenv("HOME") + "/ os.mkdir(urdf_dir_path) add_line_string = '\file://%s\' % name - manifest_path = '%s/../manifest.xml' % urdf_dir_path - if len(commands.getoutput("grep %s %s" % (add_line_string, manifest_path))) == 0: - print "[eus2urdf] add file path to manifest.xml" - os.system('sed -i -e \"s@ @ %s\\n @g\" %s' % (add_line_string, manifest_path)) + model_config_path = '%s/../model.config' % urdf_dir_path + if len(commands.getoutput("grep %s %s" % (add_line_string, model_config_path))) == 0: + print "[eus2urdf] add file path to model.config" + os.system('sed -i -e \"s@ @ %s\\n @g\" %s' % (add_line_string, model_config_path)) print "[eus2urdf] make model.config in urdf directory" config_path = '%s/model.config' % urdf_dir_path @@ -39,8 +39,7 @@ def eus2urdf_for_gazebo (name, collada_path, urdf_path = (os.getenv("HOME") + "/ os.system('sed -i -e \"1,/ \\n false<\/static>\\n <\/gazebo>\\n @@g\" %s' % urdf_path) - # os.system('sed -i -e \"s@ @ @g\" %s' % urdf_path) + #os.system('sed -i -e \"s@@@g\" %s' % urdf_path) if __name__ == '__main__': From 4c83e396f9e5bda7664c97786df9a535bf13b1e4 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 7 Apr 2014 16:29:41 +0900 Subject: [PATCH 19/38] add samples/play-domino-simulation.l --- samples/play-domino-simulation.l | 54 ++++++++++++++++++++++++++++++++ 1 file changed, 54 insertions(+) create mode 100644 samples/play-domino-simulation.l diff --git a/samples/play-domino-simulation.l b/samples/play-domino-simulation.l new file mode 100644 index 00000000..d9bc1371 --- /dev/null +++ b/samples/play-domino-simulation.l @@ -0,0 +1,54 @@ +(load "package://eusgazebo/euslisp/eusgazebo.l") + + +(defun init-domino-simulation + () + + ;; make and view eus model + (load "package://eusgazebo/samples/domino-cube-object.l") + (setq *cube1* (domino-cube)) + (setq *cube2* (domino-cube)) + (setq *cube3* (domino-cube)) + (setq *cube4* (domino-cube)) + (setq *cube5* (domino-cube)) + (objects (list *cube1* *cube2* *cube3* *cube4* *cube5*)) + + ;; overwrite mass property + (dolist (cube (list *cube1* *cube2* *cube3* *cube4* *cube5*)) + (overwrite-mass-property cube (float-vector 0 0 1000) 10000 (list 1e10 1e10 1e9))) + + ;; generate eusgazebo instance + (setq *eusgazebo-server* (instance eusgazebo :init)) + (send *eusgazebo-server* :add-model *cube1*) + (send *eusgazebo-server* :add-model *cube2*) + (send *eusgazebo-server* :add-model *cube3*) + (send *eusgazebo-server* :add-model *cube4*) + (send *eusgazebo-server* :add-model *cube5* :static t) + (unix::usleep (* 1000 1000))) + + +(defun play-domino-simulation + () + + ;; start simulation + (progn + ;; pause simulation + (send *eusgazebo-server* :pause-sim) + ;; set eus model pose + (send *cube1* :newcoords (make-coords :pos #f(-3000 0 2000) :rpy (list 0 (/ pi 20) 0))) + (send *cube2* :newcoords (make-coords :pos #f(-1500 0 2000) :rpy (list 0 0 0))) + (send *cube3* :newcoords (make-coords :pos #f(0 0 2000) :rpy (list 0 0 0))) + (send *cube4* :newcoords (make-coords :pos #f(1500 0 2000) :rpy (list 0 0 0))) + (send *cube5* :newcoords (make-coords :pos #f(3000 0 0) :rpy (list 0 0 0))) + ;; apply eus pose to gazebo + (send *eusgazebo-server* :eus2gzb) + ;; unpause simulation + (send *eusgazebo-server* :unpause-sim) + ;; view + (dotimes (i 500) + (send *eusgazebo-server* :gzb2eus) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (unix::usleep (* 10 1000))) + ;;(send *eusgazebo-server* :gzb2eus-loop) + )) From ec95b9f5d4a66a05fc6a5c8dad3e827d1ecb03b8 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 7 Apr 2014 16:38:59 +0900 Subject: [PATCH 20/38] set valid CoG, generate static model. --- euslisp/eusgazebo-util.l | 14 ++++++-- euslisp/eusgazebo.l | 6 ++-- euslisp/mass-property-util.l | 33 +++++++++++------ samples/domino-cube-object.l | 2 +- scripts/make_urdf_static_model_for_gazebo.py | 37 ++++++++++++++++++++ 5 files changed, 75 insertions(+), 17 deletions(-) create mode 100755 scripts/make_urdf_static_model_for_gazebo.py diff --git a/euslisp/eusgazebo-util.l b/euslisp/eusgazebo-util.l index bef10ba1..a26e318c 100644 --- a/euslisp/eusgazebo-util.l +++ b/euslisp/eusgazebo-util.l @@ -7,7 +7,8 @@ &key (name (send model :name)) (collada-dir "/tmp") - (urdf-dir nil)) + (urdf-dir nil) + (static nil)) (cond ((not (subclassp (class model) cascaded-link)) @@ -21,12 +22,19 @@ (cond ((probe-file collada-path) (format t "generate ~A.dae~%convert to urdf~%" name) + ;; convert to urdf model (unix:system (format nil "rosrun eusgazebo eus2urdf_for_gazebo.py ~a ~a ~a" - name collada-path (if urdf-dir urdf-dir "")))) + name collada-path (if urdf-dir urdf-dir ""))) + ;; make static model + (unix:system + (format nil + "rosrun eusgazebo make_urdf_static_model_for_gazebo.py ~a ~a" + name (if urdf-dir urdf-dir ""))) + ) (t (format nil "dae error~%")))) - (format nil "~a/~a/~a" urdf-dir name "model.urdf") + (format nil "~a/~a~a/~a" urdf-dir name (if static "_static" "") "model.urdf") ))) diff --git a/euslisp/eusgazebo.l b/euslisp/eusgazebo.l index c6c03bf6..aff640c7 100644 --- a/euslisp/eusgazebo.l +++ b/euslisp/eusgazebo.l @@ -40,14 +40,14 @@ ;; add eus model to gazebo world (:add-model (model - &key (urdf-dir "/tmp/gazebo_model")) + &key (static nil) (urdf-dir "/tmp/gazebo_model")) ;; check if already added (when (member model model-list) (return-from :add-model nil)) ;; check mass property - (overwirte-mass-property-if-too-small model) + (overwrite-mass-property-if-too-small model) ;; overwrite model name (cond @@ -68,7 +68,7 @@ ;; convert urdf (let* (urdf-model-path) - (setq urdf-model-path (irteus2urdf-for-gazebo model :name (send model :get :gazebo-model-name) :urdf-dir urdf-dir)) + (setq urdf-model-path (irteus2urdf-for-gazebo model :name (send model :get :gazebo-model-name) :urdf-dir urdf-dir :static static)) (send model :put :urdf-model-path urdf-model-path)) ;; spawn model diff --git a/euslisp/mass-property-util.l b/euslisp/mass-property-util.l index eb6696b1..ff7a4da5 100644 --- a/euslisp/mass-property-util.l +++ b/euslisp/mass-property-util.l @@ -1,13 +1,14 @@ ;; overwrite mass property ;;;;;;;;;;;;;;; -(defun overwirte-mass-property - (obj link-mass link-inertia-diag-elem) +(defun overwrite-mass-property + (obj cog link-mass link-inertia-diag-elem) (dolist (link (send obj :links)) + (overwrite-link-cog link cog) (overwrite-link-mass link link-mass) (overwrite-link-inertia link link-inertia-diag-elem)) ) -(defun overwirte-mass-property-if-too-small +(defun overwrite-mass-property-if-too-small (obj) (let* ((link-weight-min 1) ;; [g] @@ -17,19 +18,24 @@ (let* ((link-weight (slot link bodyset-link 'weight)) (link-inertia (slot (car (send obj :links)) bodyset-link 'inertia-tensor)) (link-inertia-diag (mapcar #'(lambda (i) (elt (matrix-row link-inertia i) i)) (list 0 1 2)))) - ;; overwirte mass if too small + ;; overwrite mass if too small (when (< link-weight link-weight-min) (warning-message 3 "overwrite mass because original value is too small: ~a [g] -> ~a [g]~%" link-weight link-weight-min) (overwrite-link-mass link link-weight-min)) ;; overwrite inertia if too small - (let* ((link-inertia-diag-overwirte + (let* ((link-inertia-diag-overwrite (mapcar #'max link-inertia-diag link-inertia-diag-min))) - (unless (equal link-inertia-diag link-inertia-diag-overwirte) + (unless (equal link-inertia-diag link-inertia-diag-overwrite) (warning-message 3 "overwrite inertia because original value is too small: ~a [gmm^2] -> ~a [gmm^2]~%" - link-inertia-diag link-inertia-diag-overwirte) - (overwrite-link-inertia link link-inertia-diag-overwirte))) + link-inertia-diag link-inertia-diag-overwrite) + (overwrite-link-inertia link link-inertia-diag-overwrite))) )))) +(defun overwrite-link-cog + (link cog) + + (setf (slot link bodyset-link 'acentroid) cog)) + (defun overwrite-link-mass (link link-mass) @@ -47,6 +53,11 @@ ;; get mass property ;;;;;;;;;;;;;;; +(defun get-link-cog + (link) + + (slot link bodyset-link 'acentroid)) + (defun get-link-mass (link) @@ -60,8 +71,10 @@ (defun print-obj-mass-property (obj) - (warning-message 2 "whole mass: ~a [g]~%first link inertia: ~a [gmm^2]~%" - (send obj :weight) (get-link-inertia (car (send obj :links))))) + (warning-message 2 "whole mass: ~a [g]~%first link cog: ~a[mm]~%first link inertia: ~a [gmm^2]~%" + (send obj :weight) + (get-link-cog (car (send obj :links))) + (get-link-inertia (car (send obj :links))))) ;; caluculate valid mass property automatically ;;;;;;;;;;;;;;; diff --git a/samples/domino-cube-object.l b/samples/domino-cube-object.l index d3f331c9..9c286309 100644 --- a/samples/domino-cube-object.l +++ b/samples/domino-cube-object.l @@ -29,7 +29,7 @@ )) )) (dolist (b (cdr bc)) (send (car bc) :assoc b)) - (send (elt bc 0) :set-color :gray40) + (send (elt bc 0) :set-color :gray20) (setq blink0 (instance bodyset-link :init (make-cascoords) :bodies bc :name :domino-cube-bodyset2 :weight 2000 :centroid (float-vector 0.0 0.0 0.0) :inertia-tensor #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) ;; definition of assoc diff --git a/scripts/make_urdf_static_model_for_gazebo.py b/scripts/make_urdf_static_model_for_gazebo.py new file mode 100755 index 00000000..183d2a4b --- /dev/null +++ b/scripts/make_urdf_static_model_for_gazebo.py @@ -0,0 +1,37 @@ +#! /usr/bin/env python + +import sys +import os +import commands + + +def make_static_model (name, urdf_path = (os.getenv("HOME") + "/.gazebo/models"), overwrite=True): + + urdf_dir_path = urdf_path + "/" + name + static_urdf_dir_path = urdf_dir_path + '_static' + urdf_path = urdf_dir_path + '/' + 'model.urdf' + static_urdf_path = static_urdf_dir_path + '/' + 'model.urdf' + + if overwrite: + os.system("rm -rf %s" % static_urdf_dir_path) + else: + if os.path.exists(static_urdf_dir_path): + print '[ERROR] the same name static model already exits' + exit(1) + + os.system("cp -r %s %s" % (urdf_dir_path, static_urdf_dir_path)) + + if len(commands.getoutput("grep \"false\" %s" % static_urdf_path)) != 0: + os.system('sed -i -e \"s@false@true@g\" %s' % static_urdf_path) + elif len(commands.getoutput("grep \"true\" %s" % static_urdf_path)) == 0: + os.system('sed -i -e \"s@ \\n true\\n \\n @ \\n true\\n \\n@g\" %s' % static_urdf_path) + + os.system('sed -i -e "s@ 2: + make_static_model(sys.argv[1], sys.argv[2]) + elif len(sys.argv) > 1: + make_static_model(sys.argv[1]) From 7dd068c34a72cfec304dab4c6f363870751221a0 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 7 Apr 2014 16:39:26 +0900 Subject: [PATCH 21/38] added test program for play domino --- test/test-play-domino-simulation.l | 15 +++++++++++++++ test/test-play-domino-simulation.test | 4 ++++ 2 files changed, 19 insertions(+) create mode 100755 test/test-play-domino-simulation.l create mode 100644 test/test-play-domino-simulation.test diff --git a/test/test-play-domino-simulation.l b/test/test-play-domino-simulation.l new file mode 100755 index 00000000..2984371d --- /dev/null +++ b/test/test-play-domino-simulation.l @@ -0,0 +1,15 @@ +#!/usr/bin/env roseus + +(require :unittest "lib/llib/unittest.l") + +(load "package://eusgazebo/samples/play-domino-simulation.l") + +(init-unit-test) + +(deftest test-play-domino-simulation + (init-domino-simulation) + (play-domino-simulation) + (unix:system "pkill gzserver")) + +(run-all-tests) +(exit) diff --git a/test/test-play-domino-simulation.test b/test/test-play-domino-simulation.test new file mode 100644 index 00000000..adab1da3 --- /dev/null +++ b/test/test-play-domino-simulation.test @@ -0,0 +1,4 @@ + + + From 6b9dd1f0c8791814d282df40bed2c4f737936d4d Mon Sep 17 00:00:00 2001 From: mmurooka Date: Wed, 16 Apr 2014 13:13:37 +0900 Subject: [PATCH 22/38] added pinball sample --- samples/ground-plane-object.l | 60 +++++++++++++ samples/play-pinball-simulation.l | 74 +++++++++++++++++ samples/simple-ball-object.l | 134 ++++++++++++++++++++++++++++++ 3 files changed, 268 insertions(+) create mode 100644 samples/ground-plane-object.l create mode 100644 samples/play-pinball-simulation.l create mode 100644 samples/simple-ball-object.l diff --git a/samples/ground-plane-object.l b/samples/ground-plane-object.l new file mode 100644 index 00000000..e663aa4c --- /dev/null +++ b/samples/ground-plane-object.l @@ -0,0 +1,60 @@ +;; +;; DO NOT EDIT THIS FILE +;; this file is automatically generated from euslisp+euslib version +;; +;; +(defclass ground-plane-object + :super cascaded-link + :slots (sensors + )) +(defmethod ground-plane-object + (:init + (&rest args &key (name "ground-plane") (pos (float-vector 0 0 0)) (rot (unit-matrix 3)) &allow-other-keys) + (let (c bc + blink0 + ) + (send-super* :init :name name args) + + ;; definition of link + + ;; definition of :ground-plane-bodyset2 + (setq bc (list + (instance faceset :init :faces (list + (instance face :init :vertices (list (float-vector 5000.0 3000.0 1.0) (float-vector -5000.0 3000.0 1.0) (float-vector -5000.0 -3000.0 1.0) (float-vector 5000.0 -3000.0 1.0))) + (instance face :init :vertices (list (float-vector -5000.0 3000.0 0.0) (float-vector 5000.0 3000.0 0.0) (float-vector 5000.0 -3000.0 0.0) (float-vector -5000.0 -3000.0 0.0))) + (instance face :init :vertices (list (float-vector 5000.0 3000.0 1.0) (float-vector 5000.0 3000.0 0.0) (float-vector -5000.0 3000.0 0.0) (float-vector -5000.0 3000.0 1.0))) + (instance face :init :vertices (list (float-vector 5000.0 -3000.0 1.0) (float-vector 5000.0 -3000.0 0.0) (float-vector 5000.0 3000.0 0.0) (float-vector 5000.0 3000.0 1.0))) + (instance face :init :vertices (list (float-vector -5000.0 -3000.0 1.0) (float-vector -5000.0 -3000.0 0.0) (float-vector 5000.0 -3000.0 0.0) (float-vector 5000.0 -3000.0 1.0))) + (instance face :init :vertices (list (float-vector -5000.0 3000.0 1.0) (float-vector -5000.0 3000.0 0.0) (float-vector -5000.0 -3000.0 0.0) (float-vector -5000.0 -3000.0 1.0))) + )) + )) + (dolist (b (cdr bc)) (send (car bc) :assoc b)) + (send (elt bc 0) :set-color :gray80) + (setq blink0 (instance bodyset-link :init (make-cascoords) :bodies bc :name :ground-plane-bodyset2 :weight 10000 :centroid (float-vector 0.0 0.0 0.0) :inertia-tensor #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) + + ;; definition of assoc + (send self :assoc blink0) + + ;; definition of end-coords + + ;; definition of joint + + + ;; init-ending + (setq links (list blink0)) + (setq joint-list (list)) + (send self :init-ending) + (send self :move-to (make-coords :pos pos :rot rot)) + (send-all links :worldcoords) + + self)) + (:cameras (&rest args) + (forward-message-to-all (list) args)) + + (:handle (&rest args) (forward-message-to-all (list ) args)) + (:attention (&rest args) (forward-message-to-all (list ) args)) + (:button (&rest args) (forward-message-to-all (list ) args)) + ) + +(defun ground-plane (&rest args) (instance* ground-plane-object :init args)) +;; (format *error-output* "(instance ground-plane-object :init) for generating model~%") diff --git a/samples/play-pinball-simulation.l b/samples/play-pinball-simulation.l new file mode 100644 index 00000000..0919c341 --- /dev/null +++ b/samples/play-pinball-simulation.l @@ -0,0 +1,74 @@ +(load "package://eusgazebo/euslisp/eusgazebo.l") + + +(defun init-pinball-simulation + () + + ;; make and view eus model + (load "package://eusgazebo/samples/ground-plane-object.l") + (load "package://eusgazebo/samples/simple-ball-object.l") + (load "package://eusgazebo/samples/domino-cube-object.l") + (setq *floar* (ground-plane)) + (setq *ball* (simple-ball)) + (setq *bar1* (domino-cube)) + (setq *bar2* (domino-cube)) + (setq *wall1* (domino-cube)) + (setq *wall2* (domino-cube)) + (objects (list *ball* *floar* *bar1* *bar2* *wall1* *wall2*)) + + ;; overwrite mass property + (overwrite-mass-property *ball* (float-vector 0 0 100) 1000 (list 1e8 1e8 1e8)) + + ;; generate eusgazebo instance + (setq *eusgazebo-server* (instance eusgazebo :init)) + (send *eusgazebo-server* :add-model *floar* :static t) + (send *eusgazebo-server* :add-model *wall1* :static t) + (send *eusgazebo-server* :add-model *wall2* :static t) + (send *eusgazebo-server* :add-model *bar1* :static t) + (send *eusgazebo-server* :add-model *bar2* :static t) + (send *eusgazebo-server* :add-model *ball*) + (unix::usleep (* 1000 1000))) + + +(defun play-pinball-simulation + () + + ;; start simulation + ;; pause simulation + (send *eusgazebo-server* :pause-sim) + ;; set eus model pose + (send *floar* :newcoords (make-coords :pos #f(0 0 2000) :rpy (list 0 (/ pi 10) 0))) + (send *ball* :newcoords (make-coords :pos #f(-4000 0 5000) :rpy (list 0 0 0))) + (send *wall1* :newcoords + (send (make-coords :pos #f(-3000 1500 3500) :rpy (list 0 (/ pi 10) pi/2)) :rotate (deg2rad 40) :y :local)) + (send *wall2* :newcoords + (send (make-coords :pos #f(0 -500 2500) :rpy (list 0 (/ pi 10) pi/2)) :rotate (deg2rad -40) :y :local)) + (send *bar1* :newcoords + (send (make-coords :pos #f(2000 -1900 1800) :rpy (list 0 (/ pi 10) pi/2)) :rotate 0 :y :local)) + (send *bar2* :newcoords + (send (make-coords :pos #f(2000 1900 1800) :rpy (list 0 (/ pi 10) pi/2)) :rotate pi :y :local)) + + ;; apply eus pose to gazebo + (send *eusgazebo-server* :eus2gzb) + ;; unpause simulation + (send *eusgazebo-server* :unpause-sim) + (unix::usleep (* 500 1000)) + + (do-until-key + ;; gzb2eus + (send *eusgazebo-server* :gzb2eus) + (send *irtviewer* :draw-objects) + + ;; eus2gzb + (send *bar1* :rotate (deg2rad 4) :y :local) + (send *bar2* :rotate (deg2rad -4) :y :local) + (send *eusgazebo-server* :eus2gzb *bar1*) + (send *eusgazebo-server* :eus2gzb *bar2*) + (when (< (elt (send *ball* :worldpos) 2) 1000) + (send *ball* :newcoords (make-coords :pos #f(-4000 0 5000) :rpy (list 0 0 0))) + (send *eusgazebo-server* :eus2gzb *ball*)) + + ;; view + (x::window-main-one) + (unix::usleep (* 10 1000))) + ) diff --git a/samples/simple-ball-object.l b/samples/simple-ball-object.l new file mode 100644 index 00000000..0fae22ba --- /dev/null +++ b/samples/simple-ball-object.l @@ -0,0 +1,134 @@ +;; +;; DO NOT EDIT THIS FILE +;; this file is automatically generated from euslisp+euslib version +;; +;; +(defclass simple-ball-object + :super cascaded-link + :slots (sensors + )) +(defmethod simple-ball-object + (:init + (&rest args &key (name "simple-ball") (pos (float-vector 0 0 0)) (rot (unit-matrix 3)) &allow-other-keys) + (let (c bc + blink0 + ) + (send-super* :init :name name args) + + ;; definition of link + + ;; definition of :simple-ball-bodyset2 + (setq bc (list + (instance faceset :init :faces (list + (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 154.508 250.0 404.508) (float-vector 404.508 154.508 250.0))) + (instance face :init :vertices (list (float-vector 404.508 154.508 250.0) (float-vector 425.325 262.866 0.0) (float-vector 250.0 404.508 154.508))) + (instance face :init :vertices (list (float-vector 154.508 250.0 404.508) (float-vector 262.866 0.0 425.325) (float-vector 404.508 154.508 250.0))) + (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 0.0 425.325 262.866) (float-vector 154.508 250.0 404.508))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 0.0 0.0 500.0) (float-vector -154.508 -250.0 404.508))) + (instance face :init :vertices (list (float-vector -154.508 -250.0 404.508) (float-vector 0.0 -425.325 262.866) (float-vector 154.508 -250.0 404.508))) + (instance face :init :vertices (list (float-vector 0.0 0.0 500.0) (float-vector -262.866 0.0 425.325) (float-vector -154.508 -250.0 404.508))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 262.866 0.0 425.325) (float-vector 0.0 0.0 500.0))) + (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector -250.0 404.508 154.508) (float-vector 0.0 500.0 0.0))) + (instance face :init :vertices (list (float-vector 0.0 500.0 0.0) (float-vector 0.0 425.325 -262.866) (float-vector -250.0 404.508 -154.508))) + (instance face :init :vertices (list (float-vector -250.0 404.508 154.508) (float-vector 0.0 425.325 262.866) (float-vector 0.0 500.0 0.0))) + (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector -425.325 262.866 0.0) (float-vector -250.0 404.508 154.508))) + (instance face :init :vertices (list (float-vector 154.508 250.0 404.508) (float-vector -154.508 250.0 404.508) (float-vector 0.0 0.0 500.0))) + (instance face :init :vertices (list (float-vector 0.0 0.0 500.0) (float-vector 262.866 0.0 425.325) (float-vector 154.508 250.0 404.508))) + (instance face :init :vertices (list (float-vector -154.508 250.0 404.508) (float-vector -262.866 0.0 425.325) (float-vector 0.0 0.0 500.0))) + (instance face :init :vertices (list (float-vector 154.508 250.0 404.508) (float-vector 0.0 425.325 262.866) (float-vector -154.508 250.0 404.508))) + (instance face :init :vertices (list (float-vector -250.0 404.508 154.508) (float-vector -404.508 154.508 250.0) (float-vector -154.508 250.0 404.508))) + (instance face :init :vertices (list (float-vector -154.508 250.0 404.508) (float-vector 0.0 425.325 262.866) (float-vector -250.0 404.508 154.508))) + (instance face :init :vertices (list (float-vector -404.508 154.508 250.0) (float-vector -262.866 0.0 425.325) (float-vector -154.508 250.0 404.508))) + (instance face :init :vertices (list (float-vector -250.0 404.508 154.508) (float-vector -425.325 262.866 0.0) (float-vector -404.508 154.508 250.0))) + (instance face :init :vertices (list (float-vector -404.508 154.508 250.0) (float-vector -500.0 0.0 0.0) (float-vector -404.508 -154.508 250.0))) + (instance face :init :vertices (list (float-vector -404.508 -154.508 250.0) (float-vector -262.866 0.0 425.325) (float-vector -404.508 154.508 250.0))) + (instance face :init :vertices (list (float-vector -500.0 0.0 0.0) (float-vector -425.325 -262.866 0.0) (float-vector -404.508 -154.508 250.0))) + (instance face :init :vertices (list (float-vector -404.508 154.508 250.0) (float-vector -425.325 262.866 0.0) (float-vector -500.0 0.0 0.0))) + (instance face :init :vertices (list (float-vector -404.508 154.508 -250.0) (float-vector -404.508 -154.508 -250.0) (float-vector -500.0 0.0 0.0))) + (instance face :init :vertices (list (float-vector -500.0 0.0 0.0) (float-vector -425.325 262.866 0.0) (float-vector -404.508 154.508 -250.0))) + (instance face :init :vertices (list (float-vector -404.508 -154.508 -250.0) (float-vector -425.325 -262.866 0.0) (float-vector -500.0 0.0 0.0))) + (instance face :init :vertices (list (float-vector -404.508 154.508 -250.0) (float-vector -262.866 0.0 -425.325) (float-vector -404.508 -154.508 -250.0))) + (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector -250.0 -404.508 -154.508) (float-vector -404.508 -154.508 -250.0))) + (instance face :init :vertices (list (float-vector -404.508 -154.508 -250.0) (float-vector -262.866 0.0 -425.325) (float-vector -154.508 -250.0 -404.508))) + (instance face :init :vertices (list (float-vector -250.0 -404.508 -154.508) (float-vector -425.325 -262.866 0.0) (float-vector -404.508 -154.508 -250.0))) + (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector 0.0 -425.325 -262.866) (float-vector -250.0 -404.508 -154.508))) + (instance face :init :vertices (list (float-vector -404.508 -154.508 250.0) (float-vector -250.0 -404.508 154.508) (float-vector -154.508 -250.0 404.508))) + (instance face :init :vertices (list (float-vector -154.508 -250.0 404.508) (float-vector -262.866 0.0 425.325) (float-vector -404.508 -154.508 250.0))) + (instance face :init :vertices (list (float-vector -250.0 -404.508 154.508) (float-vector 0.0 -425.325 262.866) (float-vector -154.508 -250.0 404.508))) + (instance face :init :vertices (list (float-vector -404.508 -154.508 250.0) (float-vector -425.325 -262.866 0.0) (float-vector -250.0 -404.508 154.508))) + (instance face :init :vertices (list (float-vector -250.0 -404.508 -154.508) (float-vector 0.0 -500.0 0.0) (float-vector -250.0 -404.508 154.508))) + (instance face :init :vertices (list (float-vector -250.0 -404.508 154.508) (float-vector -425.325 -262.866 0.0) (float-vector -250.0 -404.508 -154.508))) + (instance face :init :vertices (list (float-vector 0.0 -500.0 0.0) (float-vector 0.0 -425.325 262.866) (float-vector -250.0 -404.508 154.508))) + (instance face :init :vertices (list (float-vector -250.0 -404.508 -154.508) (float-vector 0.0 -425.325 -262.866) (float-vector 0.0 -500.0 0.0))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 250.0 -404.508 154.508) (float-vector 404.508 -154.508 250.0))) + (instance face :init :vertices (list (float-vector 404.508 -154.508 250.0) (float-vector 262.866 0.0 425.325) (float-vector 154.508 -250.0 404.508))) + (instance face :init :vertices (list (float-vector 250.0 -404.508 154.508) (float-vector 425.325 -262.866 0.0) (float-vector 404.508 -154.508 250.0))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 0.0 -425.325 262.866) (float-vector 250.0 -404.508 154.508))) + (instance face :init :vertices (list (float-vector 0.0 -500.0 0.0) (float-vector 250.0 -404.508 -154.508) (float-vector 250.0 -404.508 154.508))) + (instance face :init :vertices (list (float-vector 250.0 -404.508 154.508) (float-vector 0.0 -425.325 262.866) (float-vector 0.0 -500.0 0.0))) + (instance face :init :vertices (list (float-vector 250.0 -404.508 -154.508) (float-vector 425.325 -262.866 0.0) (float-vector 250.0 -404.508 154.508))) + (instance face :init :vertices (list (float-vector 0.0 -500.0 0.0) (float-vector 0.0 -425.325 -262.866) (float-vector 250.0 -404.508 -154.508))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 -404.508) (float-vector 404.508 -154.508 -250.0) (float-vector 250.0 -404.508 -154.508))) + (instance face :init :vertices (list (float-vector 250.0 -404.508 -154.508) (float-vector 0.0 -425.325 -262.866) (float-vector 154.508 -250.0 -404.508))) + (instance face :init :vertices (list (float-vector 404.508 -154.508 -250.0) (float-vector 425.325 -262.866 0.0) (float-vector 250.0 -404.508 -154.508))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 -404.508) (float-vector 262.866 0.0 -425.325) (float-vector 404.508 -154.508 -250.0))) + (instance face :init :vertices (list (float-vector 404.508 -154.508 250.0) (float-vector 500.0 0.0 0.0) (float-vector 404.508 154.508 250.0))) + (instance face :init :vertices (list (float-vector 404.508 154.508 250.0) (float-vector 262.866 0.0 425.325) (float-vector 404.508 -154.508 250.0))) + (instance face :init :vertices (list (float-vector 500.0 0.0 0.0) (float-vector 425.325 262.866 0.0) (float-vector 404.508 154.508 250.0))) + (instance face :init :vertices (list (float-vector 404.508 -154.508 250.0) (float-vector 425.325 -262.866 0.0) (float-vector 500.0 0.0 0.0))) + (instance face :init :vertices (list (float-vector 404.508 -154.508 -250.0) (float-vector 404.508 154.508 -250.0) (float-vector 500.0 0.0 0.0))) + (instance face :init :vertices (list (float-vector 500.0 0.0 0.0) (float-vector 425.325 -262.866 0.0) (float-vector 404.508 -154.508 -250.0))) + (instance face :init :vertices (list (float-vector 404.508 154.508 -250.0) (float-vector 425.325 262.866 0.0) (float-vector 500.0 0.0 0.0))) + (instance face :init :vertices (list (float-vector 404.508 -154.508 -250.0) (float-vector 262.866 0.0 -425.325) (float-vector 404.508 154.508 -250.0))) + (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 250.0 404.508 -154.508) (float-vector 0.0 500.0 0.0))) + (instance face :init :vertices (list (float-vector 0.0 500.0 0.0) (float-vector 0.0 425.325 262.866) (float-vector 250.0 404.508 154.508))) + (instance face :init :vertices (list (float-vector 250.0 404.508 -154.508) (float-vector 0.0 425.325 -262.866) (float-vector 0.0 500.0 0.0))) + (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 425.325 262.866 0.0) (float-vector 250.0 404.508 -154.508))) + (instance face :init :vertices (list (float-vector 404.508 154.508 -250.0) (float-vector 154.508 250.0 -404.508) (float-vector 250.0 404.508 -154.508))) + (instance face :init :vertices (list (float-vector 250.0 404.508 -154.508) (float-vector 425.325 262.866 0.0) (float-vector 404.508 154.508 -250.0))) + (instance face :init :vertices (list (float-vector 154.508 250.0 -404.508) (float-vector 0.0 425.325 -262.866) (float-vector 250.0 404.508 -154.508))) + (instance face :init :vertices (list (float-vector 404.508 154.508 -250.0) (float-vector 262.866 0.0 -425.325) (float-vector 154.508 250.0 -404.508))) + (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector -154.508 250.0 -404.508) (float-vector -404.508 154.508 -250.0))) + (instance face :init :vertices (list (float-vector -404.508 154.508 -250.0) (float-vector -425.325 262.866 0.0) (float-vector -250.0 404.508 -154.508))) + (instance face :init :vertices (list (float-vector -154.508 250.0 -404.508) (float-vector -262.866 0.0 -425.325) (float-vector -404.508 154.508 -250.0))) + (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector 0.0 425.325 -262.866) (float-vector -154.508 250.0 -404.508))) + (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector 0.0 0.0 -500.0) (float-vector 154.508 -250.0 -404.508))) + (instance face :init :vertices (list (float-vector 154.508 -250.0 -404.508) (float-vector 0.0 -425.325 -262.866) (float-vector -154.508 -250.0 -404.508))) + (instance face :init :vertices (list (float-vector 0.0 0.0 -500.0) (float-vector 262.866 0.0 -425.325) (float-vector 154.508 -250.0 -404.508))) + (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector -262.866 0.0 -425.325) (float-vector 0.0 0.0 -500.0))) + (instance face :init :vertices (list (float-vector 154.508 250.0 -404.508) (float-vector 0.0 0.0 -500.0) (float-vector -154.508 250.0 -404.508))) + (instance face :init :vertices (list (float-vector -154.508 250.0 -404.508) (float-vector 0.0 425.325 -262.866) (float-vector 154.508 250.0 -404.508))) + (instance face :init :vertices (list (float-vector 0.0 0.0 -500.0) (float-vector -262.866 0.0 -425.325) (float-vector -154.508 250.0 -404.508))) + (instance face :init :vertices (list (float-vector 154.508 250.0 -404.508) (float-vector 262.866 0.0 -425.325) (float-vector 0.0 0.0 -500.0))) + )) + )) + (dolist (b (cdr bc)) (send (car bc) :assoc b)) + (send (elt bc 0) :set-color :red) + (setq blink0 (instance bodyset-link :init (make-cascoords) :bodies bc :name :simple-ball-bodyset2 :weight 1000 :centroid (float-vector 0.0 0.0 0.0) :inertia-tensor #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) + + ;; definition of assoc + (send self :assoc blink0) + + ;; definition of end-coords + + ;; definition of joint + + + ;; init-ending + (setq links (list blink0)) + (setq joint-list (list)) + (send self :init-ending) + (send self :move-to (make-coords :pos pos :rot rot)) + (send-all links :worldcoords) + + self)) + (:cameras (&rest args) + (forward-message-to-all (list) args)) + + (:handle (&rest args) (forward-message-to-all (list ) args)) + (:attention (&rest args) (forward-message-to-all (list ) args)) + (:button (&rest args) (forward-message-to-all (list ) args)) + ) + +(defun simple-ball (&rest args) (instance* simple-ball-object :init args)) +;; (format *error-output* "(instance simple-ball-object :init) for generating model~%") From 57f4ca2b34f81fa85cf209c7be62e9424345ab52 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Thu, 17 Apr 2014 15:10:17 +0900 Subject: [PATCH 23/38] change repository of eusgazebo --- CMakeLists.txt => eusgazebo/CMakeLists.txt | 0 Makefile => eusgazebo/Makefile | 0 catkin.cmake => eusgazebo/catkin.cmake | 0 {euslisp => eusgazebo/euslisp}/eusgazebo-util.l | 0 {euslisp => eusgazebo/euslisp}/eusgazebo.l | 0 {euslisp => eusgazebo/euslisp}/mass-property-util.l | 0 manifest.xml => eusgazebo/manifest.xml | 0 package.xml => eusgazebo/package.xml | 0 {samples => eusgazebo/samples}/domino-cube-object.l | 0 {samples => eusgazebo/samples}/fall-arrow-object-simulation.l | 0 {samples => eusgazebo/samples}/ground-plane-object.l | 0 {samples => eusgazebo/samples}/play-domino-simulation.l | 0 {samples => eusgazebo/samples}/play-pinball-simulation.l | 0 {samples => eusgazebo/samples}/simple-ball-object.l | 0 {scripts => eusgazebo/scripts}/eus2urdf_for_gazebo.py | 0 .../scripts}/make_urdf_static_model_for_gazebo.py | 0 {test => eusgazebo/test}/test-fall-arrow-object-simulation.l | 0 {test => eusgazebo/test}/test-fall-arrow-object-simulation.test | 0 {test => eusgazebo/test}/test-play-domino-simulation.l | 0 {test => eusgazebo/test}/test-play-domino-simulation.test | 0 20 files changed, 0 insertions(+), 0 deletions(-) rename CMakeLists.txt => eusgazebo/CMakeLists.txt (100%) rename Makefile => eusgazebo/Makefile (100%) rename catkin.cmake => eusgazebo/catkin.cmake (100%) rename {euslisp => eusgazebo/euslisp}/eusgazebo-util.l (100%) rename {euslisp => eusgazebo/euslisp}/eusgazebo.l (100%) rename {euslisp => eusgazebo/euslisp}/mass-property-util.l (100%) rename manifest.xml => eusgazebo/manifest.xml (100%) rename package.xml => eusgazebo/package.xml (100%) rename {samples => eusgazebo/samples}/domino-cube-object.l (100%) rename {samples => eusgazebo/samples}/fall-arrow-object-simulation.l (100%) rename {samples => eusgazebo/samples}/ground-plane-object.l (100%) rename {samples => eusgazebo/samples}/play-domino-simulation.l (100%) rename {samples => eusgazebo/samples}/play-pinball-simulation.l (100%) rename {samples => eusgazebo/samples}/simple-ball-object.l (100%) rename {scripts => eusgazebo/scripts}/eus2urdf_for_gazebo.py (100%) rename {scripts => eusgazebo/scripts}/make_urdf_static_model_for_gazebo.py (100%) rename {test => eusgazebo/test}/test-fall-arrow-object-simulation.l (100%) rename {test => eusgazebo/test}/test-fall-arrow-object-simulation.test (100%) rename {test => eusgazebo/test}/test-play-domino-simulation.l (100%) rename {test => eusgazebo/test}/test-play-domino-simulation.test (100%) diff --git a/CMakeLists.txt b/eusgazebo/CMakeLists.txt similarity index 100% rename from CMakeLists.txt rename to eusgazebo/CMakeLists.txt diff --git a/Makefile b/eusgazebo/Makefile similarity index 100% rename from Makefile rename to eusgazebo/Makefile diff --git a/catkin.cmake b/eusgazebo/catkin.cmake similarity index 100% rename from catkin.cmake rename to eusgazebo/catkin.cmake diff --git a/euslisp/eusgazebo-util.l b/eusgazebo/euslisp/eusgazebo-util.l similarity index 100% rename from euslisp/eusgazebo-util.l rename to eusgazebo/euslisp/eusgazebo-util.l diff --git a/euslisp/eusgazebo.l b/eusgazebo/euslisp/eusgazebo.l similarity index 100% rename from euslisp/eusgazebo.l rename to eusgazebo/euslisp/eusgazebo.l diff --git a/euslisp/mass-property-util.l b/eusgazebo/euslisp/mass-property-util.l similarity index 100% rename from euslisp/mass-property-util.l rename to eusgazebo/euslisp/mass-property-util.l diff --git a/manifest.xml b/eusgazebo/manifest.xml similarity index 100% rename from manifest.xml rename to eusgazebo/manifest.xml diff --git a/package.xml b/eusgazebo/package.xml similarity index 100% rename from package.xml rename to eusgazebo/package.xml diff --git a/samples/domino-cube-object.l b/eusgazebo/samples/domino-cube-object.l similarity index 100% rename from samples/domino-cube-object.l rename to eusgazebo/samples/domino-cube-object.l diff --git a/samples/fall-arrow-object-simulation.l b/eusgazebo/samples/fall-arrow-object-simulation.l similarity index 100% rename from samples/fall-arrow-object-simulation.l rename to eusgazebo/samples/fall-arrow-object-simulation.l diff --git a/samples/ground-plane-object.l b/eusgazebo/samples/ground-plane-object.l similarity index 100% rename from samples/ground-plane-object.l rename to eusgazebo/samples/ground-plane-object.l diff --git a/samples/play-domino-simulation.l b/eusgazebo/samples/play-domino-simulation.l similarity index 100% rename from samples/play-domino-simulation.l rename to eusgazebo/samples/play-domino-simulation.l diff --git a/samples/play-pinball-simulation.l b/eusgazebo/samples/play-pinball-simulation.l similarity index 100% rename from samples/play-pinball-simulation.l rename to eusgazebo/samples/play-pinball-simulation.l diff --git a/samples/simple-ball-object.l b/eusgazebo/samples/simple-ball-object.l similarity index 100% rename from samples/simple-ball-object.l rename to eusgazebo/samples/simple-ball-object.l diff --git a/scripts/eus2urdf_for_gazebo.py b/eusgazebo/scripts/eus2urdf_for_gazebo.py similarity index 100% rename from scripts/eus2urdf_for_gazebo.py rename to eusgazebo/scripts/eus2urdf_for_gazebo.py diff --git a/scripts/make_urdf_static_model_for_gazebo.py b/eusgazebo/scripts/make_urdf_static_model_for_gazebo.py similarity index 100% rename from scripts/make_urdf_static_model_for_gazebo.py rename to eusgazebo/scripts/make_urdf_static_model_for_gazebo.py diff --git a/test/test-fall-arrow-object-simulation.l b/eusgazebo/test/test-fall-arrow-object-simulation.l similarity index 100% rename from test/test-fall-arrow-object-simulation.l rename to eusgazebo/test/test-fall-arrow-object-simulation.l diff --git a/test/test-fall-arrow-object-simulation.test b/eusgazebo/test/test-fall-arrow-object-simulation.test similarity index 100% rename from test/test-fall-arrow-object-simulation.test rename to eusgazebo/test/test-fall-arrow-object-simulation.test diff --git a/test/test-play-domino-simulation.l b/eusgazebo/test/test-play-domino-simulation.l similarity index 100% rename from test/test-play-domino-simulation.l rename to eusgazebo/test/test-play-domino-simulation.l diff --git a/test/test-play-domino-simulation.test b/eusgazebo/test/test-play-domino-simulation.test similarity index 100% rename from test/test-play-domino-simulation.test rename to eusgazebo/test/test-play-domino-simulation.test From 34ea50a77cfc81c910430f02c4eb7e0fc11cc376 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Thu, 17 Apr 2014 18:54:20 +0900 Subject: [PATCH 24/38] added README.md for instruction --- eusgazebo/README.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 eusgazebo/README.md diff --git a/eusgazebo/README.md b/eusgazebo/README.md new file mode 100644 index 00000000..a2a903e7 --- /dev/null +++ b/eusgazebo/README.md @@ -0,0 +1,41 @@ +### Usage +```lisp + + ;; initialize ;;;;;;;;;;;;;;;;;;;;;;;;;; + ;;;; init eusgazebo + (setq *eusgazebo-server* (instance eusgazebo :init)) + + ;;;; generate eus model + (setq *obj* (generate-eus-model-function)) + + ;;;; add eus mdoel to eusgazebo + (send *eusgazebo-server* :add-model *obj*) + + + ;; simulate ;;;;;;;;;;;;;;;;;;;;;;;;;; + ;;;; set simulation setting + (send *obj* :newcoords (make-coords :pos #f(0 0 1000))) + (send *eusgazebo-server* :pause-sim) + (send *eusgazebo-server* :eus2gzb) + + ;;;; start simulation + (send *eusgazebo-server* :unpause-sim) + (send *eusgazebo-server* :gzb2eus-loop) + +``` + +### Sample +```bash +roscore & +roscd eusgazebo/samples +roseus fall-arrow-object-simulation.l "(fall-arrow-object-simulation)" +roseus play-pinball-simulation.l "(progn (init-pinball-simulation) (play-pinball-simulation))" +roseus play-domino-simulation.l "(progn (init-domino-simulation) (play-domino-simulation))" +``` + +### ROS Test +```bash +rostest eusgazebo test-fall-arrow-object-simulation.test +rostest eusgazebo test-play-domino-simulation.test +``` + From 1476d6047b1cfb8b2555f09510eef48225decec5 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Thu, 17 Apr 2014 19:03:28 +0900 Subject: [PATCH 25/38] added comment to README.md --- eusgazebo/README.md | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/eusgazebo/README.md b/eusgazebo/README.md index a2a903e7..81ec5738 100644 --- a/eusgazebo/README.md +++ b/eusgazebo/README.md @@ -1,3 +1,8 @@ +# EusGazebo + +Integration of Euslisp and Gazebo simulation. +You can use Gazebo simulation more interactively from Euslisp interpreter. + ### Usage ```lisp From a74c540875c27a6f87eeb9de6f43edea2dc98bd8 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Tue, 29 Apr 2014 13:17:49 +0900 Subject: [PATCH 26/38] fix bug in overwrite-mass-property-if-too-small --- eusgazebo/euslisp/mass-property-util.l | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/eusgazebo/euslisp/mass-property-util.l b/eusgazebo/euslisp/mass-property-util.l index ff7a4da5..4e14aad0 100644 --- a/eusgazebo/euslisp/mass-property-util.l +++ b/eusgazebo/euslisp/mass-property-util.l @@ -16,7 +16,7 @@ ) (dolist (link (send obj :links)) (let* ((link-weight (slot link bodyset-link 'weight)) - (link-inertia (slot (car (send obj :links)) bodyset-link 'inertia-tensor)) + (link-inertia (slot link bodyset-link 'inertia-tensor)) (link-inertia-diag (mapcar #'(lambda (i) (elt (matrix-row link-inertia i) i)) (list 0 1 2)))) ;; overwrite mass if too small (when (< link-weight link-weight-min) From a81f8c00f029a219164cdb48830f5c917a2fcb09 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Tue, 29 Apr 2014 19:35:32 +0900 Subject: [PATCH 27/38] for travis restarting --- eusgazebo/package.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/eusgazebo/package.xml b/eusgazebo/package.xml index 9975688f..2c1bcb91 100644 --- a/eusgazebo/package.xml +++ b/eusgazebo/package.xml @@ -14,11 +14,9 @@ catkin - gazebo_ros roseus collada_tools - gazebo_ros roseus collada_tools From 1a69f99de300f56edea5c00deb2820df36b73d65 Mon Sep 17 00:00:00 2001 From: ohara Date: Mon, 5 May 2014 19:23:37 +0900 Subject: [PATCH 28/38] add play-top-samples --- eusgazebo/samples/play-top-simulation.l | 65 +++++++++++++++++++++++++ eusgazebo/samples/top-object.l | 57 ++++++++++++++++++++++ 2 files changed, 122 insertions(+) create mode 100755 eusgazebo/samples/play-top-simulation.l create mode 100644 eusgazebo/samples/top-object.l diff --git a/eusgazebo/samples/play-top-simulation.l b/eusgazebo/samples/play-top-simulation.l new file mode 100755 index 00000000..bc9961ad --- /dev/null +++ b/eusgazebo/samples/play-top-simulation.l @@ -0,0 +1,65 @@ +(load "package://eusgazebo/euslisp/eusgazebo.l") + + +(defun init-top-simulation + () + + ;; make and view eus model + (load "package://eusgazebo/samples/top-object.l") + (setq *top1* (top)) + (setq *top2* (top)) + + (objects (list *top2* *top1*)) + + ;; overwrite mass property + (dolist (top (list *top1* *top2*)) + (overwrite-mass-property top (float-vector 0 0 600) 10000 (list 1e10 1e10 1e9))) + + ;; generate eusgazebo instance + (setq *eusgazebo-server* (instance eusgazebo :init)) + (send *eusgazebo-server* :add-model *top1*) + (send *eusgazebo-server* :add-model *top2*) + + (unix::usleep (* 1000 1000))) + + +(defun play-top-simulation + () + + ;; start simulation + (progn + ;; pause simulation + (send *eusgazebo-server* :pause-sim) + ;; set eus model pose + (send *top1* :newcoords (make-coords :pos #f(2000 0 1000) :rpy (list 0 0 0))) + (send *top2* :newcoords (make-coords :pos #f(-2000 0 1000) :rpy (list 0 0 0))) + + + ;; apply eus pose to gazebo + ; (send *eusgazebo-server* :eus2gzb) + + + (let* ((msg (instance gazebo_msgs::ModelState :init))) + (send msg :model_name (send *top1* :get :gazebo-model-name)) + (send msg :pose (coords->pose (send *top1* :copy-worldcoords))) + (send msg :twist :linear :x -1) + (send msg :twist :angular :z 200) + (ros::publish "/gazebo/set_model_state" msg)) + + (let* ((msg (instance gazebo_msgs::ModelState :init))) + (send msg :model_name (send *top2* :get :gazebo-model-name)) + (send msg :pose (coords->pose (send *top2* :copy-worldcoords))) + (send msg :twist :linear :x 1) + (send msg :twist :angular :z 60) + (ros::publish "/gazebo/set_model_state" msg)) + + ;; unpause simulation + (send *eusgazebo-server* :unpause-sim) + ;; view + (dotimes (i 500) + (send *eusgazebo-server* :gzb2eus) + (send *irtviewer* :draw-objects) + (x::window-main-one) + (unix::usleep (* 10 1000))) + ;;(send *eusgazebo-server* :gzb2eus-loop) + )) diff --git a/eusgazebo/samples/top-object.l b/eusgazebo/samples/top-object.l new file mode 100644 index 00000000..92c9930d --- /dev/null +++ b/eusgazebo/samples/top-object.l @@ -0,0 +1,57 @@ +;; +;; +;; +(defclass top-object + :super cascaded-link + :slots (sensors + )) +(defmethod top-object + (:init + (&rest args &key (name "top") (pos (float-vector 0 0 0)) (rot (unit-matrix 3)) &allow-other-keys) + (let (c bc + blink0 + ) + (send-super* :init :name name args) + + ;; definition of link + + ;; definition of :top-bodyset2 + (setq bc (list + (instance faceset :init :faces (list + (instance face :init :vertices (list (float-vector 0 0 0) (float-vector 1000 1000 1000) (float-vector 1000 -1000 1000))) + (instance face :init :vertices (list (float-vector 0 0 0) (float-vector -1000 1000 1000) (float-vector 1000 1000 1000))) + (instance face :init :vertices (list (float-vector 0 0 0) (float-vector -1000 -1000 1000) (float-vector -1000 1000 1000) )) + (instance face :init :vertices (list (float-vector 0 0 0) (float-vector 1000 -1000 1000) (float-vector -1000 -1000 1000))) + (instance face :init :vertices (list (float-vector 1000 1000 1000) (float-vector -1000 1000 1000) (float-vector -1000 -1000 1000) (float-vector 1000 -1000.0 1000.0))) + )) + )) + (dolist (b (cdr bc)) (send (car bc) :assoc b)) + (send (elt bc 0) :set-color (float-vector 0.8 0.1 0.1)) + (setq blink0 (instance bodyset-link :init (make-cascoords) :bodies bc :name :top-bodyset2 :weight 2000 :centroid (float-vector 0.0 0.0 0.0) :inertia-tensor #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) + + ;; definition of assoc + (send self :assoc blink0) + + ;; definition of end-coords + + ;; definition of joint + + + ;; init-ending + (setq links (list blink0)) + (setq joint-list (list)) + (send self :init-ending) + (send self :move-to (make-coords :pos pos :rot rot)) + (send-all links :worldcoords) + + self)) + (:cameras (&rest args) + (forward-message-to-all (list) args)) + + (:handle (&rest args) (forward-message-to-all (list ) args)) + (:attention (&rest args) (forward-message-to-all (list ) args)) + (:button (&rest args) (forward-message-to-all (list ) args)) + ) + +(defun top (&rest args) (instance* top-object :init args)) +;; (format *error-output* "(instance top-object :init) for generating model~%") From fd7b49556e8126aa56a2e781f5f4303a6a68929a Mon Sep 17 00:00:00 2001 From: mmurooka Date: Wed, 7 May 2014 17:40:23 +0900 Subject: [PATCH 29/38] enable to set velocity for gazebo object --- eusgazebo/euslisp/eusgazebo.l | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/eusgazebo/euslisp/eusgazebo.l b/eusgazebo/euslisp/eusgazebo.l index aff640c7..f2262a59 100644 --- a/eusgazebo/euslisp/eusgazebo.l +++ b/eusgazebo/euslisp/eusgazebo.l @@ -115,15 +115,21 @@ ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;; apply eus state to gazebo (:eus2gzb - (&optional model) - - (cond (model - (let* ((msg (instance gazebo_msgs::ModelState :init))) - (send msg :model_name (send model :get :gazebo-model-name)) - (send msg :pose (coords->pose (send model :copy-worldcoords))) + (&key obj set-vel) + + (cond (obj + (let* ((msg (instance gazebo_msgs::ModelState :init)) + (dir (list :x :y :z))) + (send msg :model_name (send obj :get :gazebo-model-name)) + (send msg :pose (coords->pose (send obj :copy-worldcoords))) + (when set-vel + (mapcar #'(lambda (dir val) (send msg :twist :linear dir val)) + dir (concatenate cons (send obj :get :linear-vel))) + (mapcar #'(lambda (dir val) (send msg :twist :angular dir val)) + dir (concatenate cons (send obj :get :angular-vel)))) (ros::publish "/gazebo/set_model_state" msg))) (t ;; call for all drawed objects - (dolist (model model-list) (send self :eus2gzb model)))) + (dolist (model model-list) (send self :eus2gzb :obj model :set-vel set-vel)))) ) ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; ;; apply gazebo state to eus From 3296cc816349f8128cb447f3b1748f50c30bb421 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Wed, 7 May 2014 17:41:48 +0900 Subject: [PATCH 30/38] modify samples for last commit --- eusgazebo/samples/play-pinball-simulation.l | 6 +++--- eusgazebo/samples/play-top-simulation.l | 22 +++++---------------- 2 files changed, 8 insertions(+), 20 deletions(-) diff --git a/eusgazebo/samples/play-pinball-simulation.l b/eusgazebo/samples/play-pinball-simulation.l index 0919c341..94f40bef 100644 --- a/eusgazebo/samples/play-pinball-simulation.l +++ b/eusgazebo/samples/play-pinball-simulation.l @@ -62,11 +62,11 @@ ;; eus2gzb (send *bar1* :rotate (deg2rad 4) :y :local) (send *bar2* :rotate (deg2rad -4) :y :local) - (send *eusgazebo-server* :eus2gzb *bar1*) - (send *eusgazebo-server* :eus2gzb *bar2*) + (send *eusgazebo-server* :eus2gzb :obj *bar1*) + (send *eusgazebo-server* :eus2gzb :obj *bar2*) (when (< (elt (send *ball* :worldpos) 2) 1000) (send *ball* :newcoords (make-coords :pos #f(-4000 0 5000) :rpy (list 0 0 0))) - (send *eusgazebo-server* :eus2gzb *ball*)) + (send *eusgazebo-server* :eus2gzb :obj *ball*)) ;; view (x::window-main-one) diff --git a/eusgazebo/samples/play-top-simulation.l b/eusgazebo/samples/play-top-simulation.l index bc9961ad..9c5b5989 100755 --- a/eusgazebo/samples/play-top-simulation.l +++ b/eusgazebo/samples/play-top-simulation.l @@ -33,25 +33,13 @@ ;; set eus model pose (send *top1* :newcoords (make-coords :pos #f(2000 0 1000) :rpy (list 0 0 0))) (send *top2* :newcoords (make-coords :pos #f(-2000 0 1000) :rpy (list 0 0 0))) - - ;; apply eus pose to gazebo - ; (send *eusgazebo-server* :eus2gzb) + (send *top1* :put :linear-vel (float-vector -1 0 0)) + (send *top1* :put :angular-vel (float-vector 0 0 200)) + (send *top2* :put :linear-vel (float-vector 1 0 0)) + (send *top2* :put :angular-vel (float-vector 0 0 60)) - - (let* ((msg (instance gazebo_msgs::ModelState :init))) - (send msg :model_name (send *top1* :get :gazebo-model-name)) - (send msg :pose (coords->pose (send *top1* :copy-worldcoords))) - (send msg :twist :linear :x -1) - (send msg :twist :angular :z 200) - (ros::publish "/gazebo/set_model_state" msg)) - - (let* ((msg (instance gazebo_msgs::ModelState :init))) - (send msg :model_name (send *top2* :get :gazebo-model-name)) - (send msg :pose (coords->pose (send *top2* :copy-worldcoords))) - (send msg :twist :linear :x 1) - (send msg :twist :angular :z 60) - (ros::publish "/gazebo/set_model_state" msg)) + (send *eusgazebo-server* :eus2gzb :set-vel t) ;; unpause simulation (send *eusgazebo-server* :unpause-sim) From d969721f93a249340ad4aa6f64ca32276c571a01 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Thu, 8 May 2014 16:19:35 +0900 Subject: [PATCH 31/38] added dependency to gazebo_msgs --- eusgazebo/manifest.xml | 3 ++- eusgazebo/package.xml | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/eusgazebo/manifest.xml b/eusgazebo/manifest.xml index 0555edf8..04912752 100644 --- a/eusgazebo/manifest.xml +++ b/eusgazebo/manifest.xml @@ -10,7 +10,8 @@ + - + diff --git a/eusgazebo/package.xml b/eusgazebo/package.xml index 2c1bcb91..3ebd04de 100644 --- a/eusgazebo/package.xml +++ b/eusgazebo/package.xml @@ -15,9 +15,11 @@ catkin roseus - collada_tools + collada_urdf_jsk_patch + gazebo_msgs roseus - collada_tools + collada_urdf_jsk_patch + gazebo_msgs \ No newline at end of file From 1d4f641559b4e59e1968eae6ade7b09dd55d6aaf Mon Sep 17 00:00:00 2001 From: mmurooka Date: Thu, 8 May 2014 16:50:12 +0900 Subject: [PATCH 32/38] use collada_to_urdf of collada_urdf_jsk_patch --- eusgazebo/scripts/eus2urdf_for_gazebo.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/eusgazebo/scripts/eus2urdf_for_gazebo.py b/eusgazebo/scripts/eus2urdf_for_gazebo.py index e8b07946..af81b2c1 100755 --- a/eusgazebo/scripts/eus2urdf_for_gazebo.py +++ b/eusgazebo/scripts/eus2urdf_for_gazebo.py @@ -33,7 +33,7 @@ def eus2urdf_for_gazebo (name, collada_path, urdf_path = (os.getenv("HOME") + "/ meshes_path = urdf_dir_path + '/meshes' urdf_path = urdf_dir_path + '/' + 'model.urdf' os.mkdir(meshes_path) - os.system('rosrun collada_tools collada_to_urdf %s -G -A --mesh_output_dir %s --mesh_prefix "model://%s/meshes" -O %s' % (collada_path, meshes_path, name, urdf_path)) + os.system('rosrun collada_urdf_jsk_patch collada_to_urdf %s -G -A --mesh_output_dir %s --mesh_prefix "model://%s/meshes" -O %s' % (collada_path, meshes_path, name, urdf_path)) os.system('sed -i -e "s@continuous@revolute@g" %s' % urdf_path) os.system('sed -i -e \"s@\\n false<\/static>\\n <\/gazebo>\\n Date: Fri, 9 May 2014 11:27:06 +0900 Subject: [PATCH 33/38] add macro for generating message --- eusgazebo/catkin.cmake | 4 ++-- eusgazebo/package.xml | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/eusgazebo/catkin.cmake b/eusgazebo/catkin.cmake index dbd5ab1f..7b7b793e 100644 --- a/eusgazebo/catkin.cmake +++ b/eusgazebo/catkin.cmake @@ -1,9 +1,9 @@ cmake_minimum_required(VERSION 2.8.3) project(eusgazebo) -catkin_package() +find_package(catkin REQUIRED COMPONENTS message_generation gazebo_msgs rostest) -find_package(catkin REQUIRED COMPONENTS rostest) +catkin_package(CATKIN_DEPENDS message_runtime gazebo_msgs) ## Install ## install(DIRECTORY euslisp test scripts samples diff --git a/eusgazebo/package.xml b/eusgazebo/package.xml index 3ebd04de..b0690a0a 100644 --- a/eusgazebo/package.xml +++ b/eusgazebo/package.xml @@ -17,9 +17,11 @@ roseus collada_urdf_jsk_patch gazebo_msgs + message_generation roseus collada_urdf_jsk_patch gazebo_msgs + message_runtime \ No newline at end of file From 4770a6e19176649f6c0e6852986abcab44648b10 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sat, 10 May 2014 03:12:56 +0900 Subject: [PATCH 34/38] move simple-ball-object to euslisp/eus/models and load that. --- eusgazebo/samples/play-pinball-simulation.l | 2 +- eusgazebo/samples/simple-ball-object.l | 134 -------------------- 2 files changed, 1 insertion(+), 135 deletions(-) delete mode 100644 eusgazebo/samples/simple-ball-object.l diff --git a/eusgazebo/samples/play-pinball-simulation.l b/eusgazebo/samples/play-pinball-simulation.l index 94f40bef..597917d2 100644 --- a/eusgazebo/samples/play-pinball-simulation.l +++ b/eusgazebo/samples/play-pinball-simulation.l @@ -6,8 +6,8 @@ ;; make and view eus model (load "package://eusgazebo/samples/ground-plane-object.l") - (load "package://eusgazebo/samples/simple-ball-object.l") (load "package://eusgazebo/samples/domino-cube-object.l") + (load "models/simple-ball-object.l") (setq *floar* (ground-plane)) (setq *ball* (simple-ball)) (setq *bar1* (domino-cube)) diff --git a/eusgazebo/samples/simple-ball-object.l b/eusgazebo/samples/simple-ball-object.l deleted file mode 100644 index 0fae22ba..00000000 --- a/eusgazebo/samples/simple-ball-object.l +++ /dev/null @@ -1,134 +0,0 @@ -;; -;; DO NOT EDIT THIS FILE -;; this file is automatically generated from euslisp+euslib version -;; -;; -(defclass simple-ball-object - :super cascaded-link - :slots (sensors - )) -(defmethod simple-ball-object - (:init - (&rest args &key (name "simple-ball") (pos (float-vector 0 0 0)) (rot (unit-matrix 3)) &allow-other-keys) - (let (c bc - blink0 - ) - (send-super* :init :name name args) - - ;; definition of link - - ;; definition of :simple-ball-bodyset2 - (setq bc (list - (instance faceset :init :faces (list - (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 154.508 250.0 404.508) (float-vector 404.508 154.508 250.0))) - (instance face :init :vertices (list (float-vector 404.508 154.508 250.0) (float-vector 425.325 262.866 0.0) (float-vector 250.0 404.508 154.508))) - (instance face :init :vertices (list (float-vector 154.508 250.0 404.508) (float-vector 262.866 0.0 425.325) (float-vector 404.508 154.508 250.0))) - (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 0.0 425.325 262.866) (float-vector 154.508 250.0 404.508))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 0.0 0.0 500.0) (float-vector -154.508 -250.0 404.508))) - (instance face :init :vertices (list (float-vector -154.508 -250.0 404.508) (float-vector 0.0 -425.325 262.866) (float-vector 154.508 -250.0 404.508))) - (instance face :init :vertices (list (float-vector 0.0 0.0 500.0) (float-vector -262.866 0.0 425.325) (float-vector -154.508 -250.0 404.508))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 262.866 0.0 425.325) (float-vector 0.0 0.0 500.0))) - (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector -250.0 404.508 154.508) (float-vector 0.0 500.0 0.0))) - (instance face :init :vertices (list (float-vector 0.0 500.0 0.0) (float-vector 0.0 425.325 -262.866) (float-vector -250.0 404.508 -154.508))) - (instance face :init :vertices (list (float-vector -250.0 404.508 154.508) (float-vector 0.0 425.325 262.866) (float-vector 0.0 500.0 0.0))) - (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector -425.325 262.866 0.0) (float-vector -250.0 404.508 154.508))) - (instance face :init :vertices (list (float-vector 154.508 250.0 404.508) (float-vector -154.508 250.0 404.508) (float-vector 0.0 0.0 500.0))) - (instance face :init :vertices (list (float-vector 0.0 0.0 500.0) (float-vector 262.866 0.0 425.325) (float-vector 154.508 250.0 404.508))) - (instance face :init :vertices (list (float-vector -154.508 250.0 404.508) (float-vector -262.866 0.0 425.325) (float-vector 0.0 0.0 500.0))) - (instance face :init :vertices (list (float-vector 154.508 250.0 404.508) (float-vector 0.0 425.325 262.866) (float-vector -154.508 250.0 404.508))) - (instance face :init :vertices (list (float-vector -250.0 404.508 154.508) (float-vector -404.508 154.508 250.0) (float-vector -154.508 250.0 404.508))) - (instance face :init :vertices (list (float-vector -154.508 250.0 404.508) (float-vector 0.0 425.325 262.866) (float-vector -250.0 404.508 154.508))) - (instance face :init :vertices (list (float-vector -404.508 154.508 250.0) (float-vector -262.866 0.0 425.325) (float-vector -154.508 250.0 404.508))) - (instance face :init :vertices (list (float-vector -250.0 404.508 154.508) (float-vector -425.325 262.866 0.0) (float-vector -404.508 154.508 250.0))) - (instance face :init :vertices (list (float-vector -404.508 154.508 250.0) (float-vector -500.0 0.0 0.0) (float-vector -404.508 -154.508 250.0))) - (instance face :init :vertices (list (float-vector -404.508 -154.508 250.0) (float-vector -262.866 0.0 425.325) (float-vector -404.508 154.508 250.0))) - (instance face :init :vertices (list (float-vector -500.0 0.0 0.0) (float-vector -425.325 -262.866 0.0) (float-vector -404.508 -154.508 250.0))) - (instance face :init :vertices (list (float-vector -404.508 154.508 250.0) (float-vector -425.325 262.866 0.0) (float-vector -500.0 0.0 0.0))) - (instance face :init :vertices (list (float-vector -404.508 154.508 -250.0) (float-vector -404.508 -154.508 -250.0) (float-vector -500.0 0.0 0.0))) - (instance face :init :vertices (list (float-vector -500.0 0.0 0.0) (float-vector -425.325 262.866 0.0) (float-vector -404.508 154.508 -250.0))) - (instance face :init :vertices (list (float-vector -404.508 -154.508 -250.0) (float-vector -425.325 -262.866 0.0) (float-vector -500.0 0.0 0.0))) - (instance face :init :vertices (list (float-vector -404.508 154.508 -250.0) (float-vector -262.866 0.0 -425.325) (float-vector -404.508 -154.508 -250.0))) - (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector -250.0 -404.508 -154.508) (float-vector -404.508 -154.508 -250.0))) - (instance face :init :vertices (list (float-vector -404.508 -154.508 -250.0) (float-vector -262.866 0.0 -425.325) (float-vector -154.508 -250.0 -404.508))) - (instance face :init :vertices (list (float-vector -250.0 -404.508 -154.508) (float-vector -425.325 -262.866 0.0) (float-vector -404.508 -154.508 -250.0))) - (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector 0.0 -425.325 -262.866) (float-vector -250.0 -404.508 -154.508))) - (instance face :init :vertices (list (float-vector -404.508 -154.508 250.0) (float-vector -250.0 -404.508 154.508) (float-vector -154.508 -250.0 404.508))) - (instance face :init :vertices (list (float-vector -154.508 -250.0 404.508) (float-vector -262.866 0.0 425.325) (float-vector -404.508 -154.508 250.0))) - (instance face :init :vertices (list (float-vector -250.0 -404.508 154.508) (float-vector 0.0 -425.325 262.866) (float-vector -154.508 -250.0 404.508))) - (instance face :init :vertices (list (float-vector -404.508 -154.508 250.0) (float-vector -425.325 -262.866 0.0) (float-vector -250.0 -404.508 154.508))) - (instance face :init :vertices (list (float-vector -250.0 -404.508 -154.508) (float-vector 0.0 -500.0 0.0) (float-vector -250.0 -404.508 154.508))) - (instance face :init :vertices (list (float-vector -250.0 -404.508 154.508) (float-vector -425.325 -262.866 0.0) (float-vector -250.0 -404.508 -154.508))) - (instance face :init :vertices (list (float-vector 0.0 -500.0 0.0) (float-vector 0.0 -425.325 262.866) (float-vector -250.0 -404.508 154.508))) - (instance face :init :vertices (list (float-vector -250.0 -404.508 -154.508) (float-vector 0.0 -425.325 -262.866) (float-vector 0.0 -500.0 0.0))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 250.0 -404.508 154.508) (float-vector 404.508 -154.508 250.0))) - (instance face :init :vertices (list (float-vector 404.508 -154.508 250.0) (float-vector 262.866 0.0 425.325) (float-vector 154.508 -250.0 404.508))) - (instance face :init :vertices (list (float-vector 250.0 -404.508 154.508) (float-vector 425.325 -262.866 0.0) (float-vector 404.508 -154.508 250.0))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 404.508) (float-vector 0.0 -425.325 262.866) (float-vector 250.0 -404.508 154.508))) - (instance face :init :vertices (list (float-vector 0.0 -500.0 0.0) (float-vector 250.0 -404.508 -154.508) (float-vector 250.0 -404.508 154.508))) - (instance face :init :vertices (list (float-vector 250.0 -404.508 154.508) (float-vector 0.0 -425.325 262.866) (float-vector 0.0 -500.0 0.0))) - (instance face :init :vertices (list (float-vector 250.0 -404.508 -154.508) (float-vector 425.325 -262.866 0.0) (float-vector 250.0 -404.508 154.508))) - (instance face :init :vertices (list (float-vector 0.0 -500.0 0.0) (float-vector 0.0 -425.325 -262.866) (float-vector 250.0 -404.508 -154.508))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 -404.508) (float-vector 404.508 -154.508 -250.0) (float-vector 250.0 -404.508 -154.508))) - (instance face :init :vertices (list (float-vector 250.0 -404.508 -154.508) (float-vector 0.0 -425.325 -262.866) (float-vector 154.508 -250.0 -404.508))) - (instance face :init :vertices (list (float-vector 404.508 -154.508 -250.0) (float-vector 425.325 -262.866 0.0) (float-vector 250.0 -404.508 -154.508))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 -404.508) (float-vector 262.866 0.0 -425.325) (float-vector 404.508 -154.508 -250.0))) - (instance face :init :vertices (list (float-vector 404.508 -154.508 250.0) (float-vector 500.0 0.0 0.0) (float-vector 404.508 154.508 250.0))) - (instance face :init :vertices (list (float-vector 404.508 154.508 250.0) (float-vector 262.866 0.0 425.325) (float-vector 404.508 -154.508 250.0))) - (instance face :init :vertices (list (float-vector 500.0 0.0 0.0) (float-vector 425.325 262.866 0.0) (float-vector 404.508 154.508 250.0))) - (instance face :init :vertices (list (float-vector 404.508 -154.508 250.0) (float-vector 425.325 -262.866 0.0) (float-vector 500.0 0.0 0.0))) - (instance face :init :vertices (list (float-vector 404.508 -154.508 -250.0) (float-vector 404.508 154.508 -250.0) (float-vector 500.0 0.0 0.0))) - (instance face :init :vertices (list (float-vector 500.0 0.0 0.0) (float-vector 425.325 -262.866 0.0) (float-vector 404.508 -154.508 -250.0))) - (instance face :init :vertices (list (float-vector 404.508 154.508 -250.0) (float-vector 425.325 262.866 0.0) (float-vector 500.0 0.0 0.0))) - (instance face :init :vertices (list (float-vector 404.508 -154.508 -250.0) (float-vector 262.866 0.0 -425.325) (float-vector 404.508 154.508 -250.0))) - (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 250.0 404.508 -154.508) (float-vector 0.0 500.0 0.0))) - (instance face :init :vertices (list (float-vector 0.0 500.0 0.0) (float-vector 0.0 425.325 262.866) (float-vector 250.0 404.508 154.508))) - (instance face :init :vertices (list (float-vector 250.0 404.508 -154.508) (float-vector 0.0 425.325 -262.866) (float-vector 0.0 500.0 0.0))) - (instance face :init :vertices (list (float-vector 250.0 404.508 154.508) (float-vector 425.325 262.866 0.0) (float-vector 250.0 404.508 -154.508))) - (instance face :init :vertices (list (float-vector 404.508 154.508 -250.0) (float-vector 154.508 250.0 -404.508) (float-vector 250.0 404.508 -154.508))) - (instance face :init :vertices (list (float-vector 250.0 404.508 -154.508) (float-vector 425.325 262.866 0.0) (float-vector 404.508 154.508 -250.0))) - (instance face :init :vertices (list (float-vector 154.508 250.0 -404.508) (float-vector 0.0 425.325 -262.866) (float-vector 250.0 404.508 -154.508))) - (instance face :init :vertices (list (float-vector 404.508 154.508 -250.0) (float-vector 262.866 0.0 -425.325) (float-vector 154.508 250.0 -404.508))) - (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector -154.508 250.0 -404.508) (float-vector -404.508 154.508 -250.0))) - (instance face :init :vertices (list (float-vector -404.508 154.508 -250.0) (float-vector -425.325 262.866 0.0) (float-vector -250.0 404.508 -154.508))) - (instance face :init :vertices (list (float-vector -154.508 250.0 -404.508) (float-vector -262.866 0.0 -425.325) (float-vector -404.508 154.508 -250.0))) - (instance face :init :vertices (list (float-vector -250.0 404.508 -154.508) (float-vector 0.0 425.325 -262.866) (float-vector -154.508 250.0 -404.508))) - (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector 0.0 0.0 -500.0) (float-vector 154.508 -250.0 -404.508))) - (instance face :init :vertices (list (float-vector 154.508 -250.0 -404.508) (float-vector 0.0 -425.325 -262.866) (float-vector -154.508 -250.0 -404.508))) - (instance face :init :vertices (list (float-vector 0.0 0.0 -500.0) (float-vector 262.866 0.0 -425.325) (float-vector 154.508 -250.0 -404.508))) - (instance face :init :vertices (list (float-vector -154.508 -250.0 -404.508) (float-vector -262.866 0.0 -425.325) (float-vector 0.0 0.0 -500.0))) - (instance face :init :vertices (list (float-vector 154.508 250.0 -404.508) (float-vector 0.0 0.0 -500.0) (float-vector -154.508 250.0 -404.508))) - (instance face :init :vertices (list (float-vector -154.508 250.0 -404.508) (float-vector 0.0 425.325 -262.866) (float-vector 154.508 250.0 -404.508))) - (instance face :init :vertices (list (float-vector 0.0 0.0 -500.0) (float-vector -262.866 0.0 -425.325) (float-vector -154.508 250.0 -404.508))) - (instance face :init :vertices (list (float-vector 154.508 250.0 -404.508) (float-vector 262.866 0.0 -425.325) (float-vector 0.0 0.0 -500.0))) - )) - )) - (dolist (b (cdr bc)) (send (car bc) :assoc b)) - (send (elt bc 0) :set-color :red) - (setq blink0 (instance bodyset-link :init (make-cascoords) :bodies bc :name :simple-ball-bodyset2 :weight 1000 :centroid (float-vector 0.0 0.0 0.0) :inertia-tensor #2f((1.0 0.0 0.0) (0.0 1.0 0.0) (0.0 0.0 1.0)))) - - ;; definition of assoc - (send self :assoc blink0) - - ;; definition of end-coords - - ;; definition of joint - - - ;; init-ending - (setq links (list blink0)) - (setq joint-list (list)) - (send self :init-ending) - (send self :move-to (make-coords :pos pos :rot rot)) - (send-all links :worldcoords) - - self)) - (:cameras (&rest args) - (forward-message-to-all (list) args)) - - (:handle (&rest args) (forward-message-to-all (list ) args)) - (:attention (&rest args) (forward-message-to-all (list ) args)) - (:button (&rest args) (forward-message-to-all (list ) args)) - ) - -(defun simple-ball (&rest args) (instance* simple-ball-object :init args)) -;; (format *error-output* "(instance simple-ball-object :init) for generating model~%") From 98416d36a98655c1ec6d5594926c3727df733f9e Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sat, 10 May 2014 03:17:25 +0900 Subject: [PATCH 35/38] added rostest line. this is for temporary debug. --- .travis.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.travis.yml b/.travis.yml index 5a628275..62e1503c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,6 +73,8 @@ script: # All commands must exit with code 0 on success. Anything else is consid # for catkin - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then export TARGET_PKG=`find build/$REPOSITORY_NAME -name Makefile -print | sed s@.*/\\\\\([^\/]*\\\\\)/Makefile@\\\1@g` ; fi + + - if [ $BUILDER == catkin ]; then rostest -t eusgazebo test-fall-arrow-object-simulation.test ; fi - if [ $BUILDER == catkin -a "$TARGET_PKG" != "" ]; then catkin_make test --pkg $TARGET_PKG -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then find build -name LastTest.log -exec echo "==== {} ====" \; -exec cat {} \; ; fi - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 install > /dev/null ; fi From 4805edda60e4d0b276940f9df910e49cbacbd03d Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sat, 10 May 2014 11:25:20 +0900 Subject: [PATCH 36/38] fixed last commit --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 62e1503c..28c40d50 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,7 +73,7 @@ script: # All commands must exit with code 0 on success. Anything else is consid # for catkin - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then export TARGET_PKG=`find build/$REPOSITORY_NAME -name Makefile -print | sed s@.*/\\\\\([^\/]*\\\\\)/Makefile@\\\1@g` ; fi - + - if [ $BUILDER == catkin ]; then source devel/setup.bash ; fi - if [ $BUILDER == catkin ]; then rostest -t eusgazebo test-fall-arrow-object-simulation.test ; fi - if [ $BUILDER == catkin -a "$TARGET_PKG" != "" ]; then catkin_make test --pkg $TARGET_PKG -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then find build -name LastTest.log -exec echo "==== {} ====" \; -exec cat {} \; ; fi From 90b4ff4682ae7f705378ad682e68b30958298286 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Sun, 11 May 2014 09:20:27 +0900 Subject: [PATCH 37/38] do ls for gazebo_msgs message --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index 28c40d50..59bf3c20 100644 --- a/.travis.yml +++ b/.travis.yml @@ -74,6 +74,7 @@ script: # All commands must exit with code 0 on success. Anything else is consid - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then export TARGET_PKG=`find build/$REPOSITORY_NAME -name Makefile -print | sed s@.*/\\\\\([^\/]*\\\\\)/Makefile@\\\1@g` ; fi - if [ $BUILDER == catkin ]; then source devel/setup.bash ; fi + - if [ $BUILDER == catkin ]; then ls /home/travis/.ros/roseus/hydro/gazebo_msgs/* ; fi - if [ $BUILDER == catkin ]; then rostest -t eusgazebo test-fall-arrow-object-simulation.test ; fi - if [ $BUILDER == catkin -a "$TARGET_PKG" != "" ]; then catkin_make test --pkg $TARGET_PKG -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then find build -name LastTest.log -exec echo "==== {} ====" \; -exec cat {} \; ; fi From 8705d66139fd24a92b04fd997b40285240a1f489 Mon Sep 17 00:00:00 2001 From: mmurooka Date: Mon, 12 May 2014 02:09:42 +0900 Subject: [PATCH 38/38] find ~/.ros/roseus/hydro for debug --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 59bf3c20..3fd2d63c 100644 --- a/.travis.yml +++ b/.travis.yml @@ -74,7 +74,7 @@ script: # All commands must exit with code 0 on success. Anything else is consid - if [ $BUILDER == catkin ]; then catkin_make -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then export TARGET_PKG=`find build/$REPOSITORY_NAME -name Makefile -print | sed s@.*/\\\\\([^\/]*\\\\\)/Makefile@\\\1@g` ; fi - if [ $BUILDER == catkin ]; then source devel/setup.bash ; fi - - if [ $BUILDER == catkin ]; then ls /home/travis/.ros/roseus/hydro/gazebo_msgs/* ; fi + - if [ $BUILDER == catkin ]; then find /home/travis/.ros/roseus/hydro ; fi - if [ $BUILDER == catkin ]; then rostest -t eusgazebo test-fall-arrow-object-simulation.test ; fi - if [ $BUILDER == catkin -a "$TARGET_PKG" != "" ]; then catkin_make test --pkg $TARGET_PKG -j8 -l8 ; fi - if [ $BUILDER == catkin ]; then find build -name LastTest.log -exec echo "==== {} ====" \; -exec cat {} \; ; fi