Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Handling demo red ball in gazebo #32

Merged
merged 3 commits into from
Apr 21, 2021
Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 16 additions & 0 deletions demoRedBall/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,21 @@ add_executable(${PROJECT_NAME} ${sources})
target_link_libraries(${PROJECT_NAME} ctrlLib ${YARP_LIBRARIES})
install(TARGETS ${PROJECT_NAME} DESTINATION bin)

# world
option(GAZEBO_AVAILABLE "enable gazebo" ON)
if(GAZEBO_AVAILABLE)
find_package(GAZEBO REQUIRED)
if (GAZEBO_FOUND)
add_library(${PROJECT_NAME}-world SHARED src/world.cpp)
target_compile_definitions(${PROJECT_NAME}-world PRIVATE _USE_MATH_DEFINES)
target_include_directories(${PROJECT_NAME}-world PRIVATE ${GAZEBO_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}-world PRIVATE ${GAZEBO_LIBRARIES} ${YARP_LIBRARIES})
install(TARGETS ${PROJECT_NAME}-world LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" COMPONENT shlib)
add_subdirectory(gazebo)
endif()
endif()


if(NOT BUILD_BUNDLE)
icubcontrib_add_uninstall_target()
endif()
Expand All @@ -33,3 +48,4 @@ file(GLOB scripts ${CMAKE_CURRENT_SOURCE_DIR}/app/scripts/*.template

yarp_install(FILES ${conf} DESTINATION ${ICUBCONTRIB_CONTEXTS_INSTALL_DIR}/${PROJECT_NAME})
yarp_install(FILES ${scripts} DESTINATION ${ICUBCONTRIB_APPLICATIONS_TEMPLATES_INSTALL_DIR})

53 changes: 53 additions & 0 deletions demoRedBall/app/conf/config-gazebo.ini
Original file line number Diff line number Diff line change
@@ -0,0 +1,53 @@
[general]
robot icubSim
thread_period 20
left_arm on
right_arm on
traj_time 1.0
reach_tol 0.01
eye left
idle_tmo 5.0
use_network off
network network.ini
speech on
simulation on

[torso]
pitch on (min -10.0) (max 10.0)
roll off
yaw on (min -30.0) (max 30.0)

[left_arm]
grasp_enable on
reach_offset 0.19 -0.06 0.07
grasp_offset 0.19 0.02 0.06
hand_orientation 0.0 -0.707106781186547 0.707106781186548 3.14159265358979
impedance_velocity_mode on
impedance_stiffness 0.4 0.4 0.4 0.2 0.2
impedance_damping 0.002 0.002 0.002 0.002 0.0

[right_arm]
grasp_enable on
reach_offset 0.19 -0.01 0.08
grasp_offset 0.19 -0.02 0.08
hand_orientation 0.0 -0.707106781186547 0.707106781186548 3.14159265358979
impedance_velocity_mode on
impedance_stiffness 0.4 0.4 0.4 0.2 0.2
impedance_damping 0.002 0.002 0.002 0.002 0.0

[home_arm]
poss -30.0 30.0 0.0 45.0 0.0 0.0 0.0
vels 10.0 10.0 10.0 10.0 10.0 10.0 10.0

[arm_selection]
hysteresis_thres 0.10

[grasp]
sphere_radius 0.1
sphere_tmo 2.0
release_tmo 4.0
open_hand 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0 0.0
close_hand 10.0 80.0 30.0 30.0 30.0 40.0 30.0 40.0 150.0
vels_hand 20.0 40.0 50.0 50.0 50.0 50.0 50.0 50.0 80.0

[include speech "speech_English.ini"]
2 changes: 1 addition & 1 deletion demoRedBall/app/conf/config.ini
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

[general]
robot icub
thread_period 20
Expand All @@ -11,6 +10,7 @@ idle_tmo 5.0
use_network off
network network.ini
speech on
simulation off

[torso]
pitch on (min -10.0) (max 10.0)
Expand Down
122 changes: 122 additions & 0 deletions demoRedBall/app/scripts/demoRedBall_gazebo.xml.template
Original file line number Diff line number Diff line change
@@ -0,0 +1,122 @@
<application>
<name>Red-Ball Demo Gazebo</name>

<dependencies>
<port>/icubSim/left/out</port>
<port>/iKinGazeCtrl/rpc</port>
<port>/icubSim/cartesianController/right_arm/state:o</port>
<port>/icubSim/cartesianController/left_arm/state:o</port>
<port>/wholeBodyDynamics/right_arm/FT:i</port>
<port>/wholeBodyDynamics/left_arm/FT:i</port>
</dependencies>

<module>
<name>gzserver</name>
<parameters>grasp-ball-gazebo.sdf</parameters>
<node>localhost</node>
</module>

<module>
<name>gzclient</name>
<node>localhost</node>
</module>

<module>
<name>iKinGazeCtrl</name>
<parameters>--context gazeboCartesianControl --from iKinGazeCtrl.ini</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/head/state:o</port>
<port timeout="20">/icubSim/inertial</port>
</dependencies>
<ensure>
<wait when="stop">5</wait>
</ensure>
<node>localhost</node>
</module>

<module>
<name>yarprobotinterface</name>
<parameters>--context gazeboCartesianControl --config no_legs.xml</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/left_arm/state:o</port>
<port timeout="20">/icubSim/right_arm/state:o</port>
</dependencies>
<ensure>
<wait when="stop">5</wait>
</ensure>
<node>localhost</node>
</module>

<module>
<name>iKinCartesianSolver</name>
<parameters>--context gazeboCartesianControl --part right_arm</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/right_arm/state:o</port>
</dependencies>
<node>localhost</node>
</module>

<module>
<name>iKinCartesianSolver</name>
<parameters>--context gazeboCartesianControl --part left_arm</parameters>
<dependencies>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/left_arm/state:o</port>
</dependencies>
<node>localhost</node>
</module>

<module>
<name>wholeBodyDynamics</name>
<parameters>--robot icubSim --autoconnect --dummy_ft --headV2 --no_legs</parameters>
<dependencies>
<port timeout="20">/icubSim/head/state:o</port>
<port timeout="20">/icubSim/torso/state:o</port>
<port timeout="20">/icubSim/right_arm/state:o</port>
<port timeout="20">/icubSim/left_arm/state:o</port>
<port timeout="20">/icubSim/inertial</port>
</dependencies>
<node>localhost</node>
</module>


<module>
<name>pf3dTracker</name>
<parameters>--from pf3dTracker-gazebo.ini</parameters>
<node>pwrNode1</node>
</module>

<module>
<name>demoRedBall</name>
<parameters>--from config-gazebo.ini</parameters>
<dependencies>
<port timeout="20">/iKinGazeCtrl/rpc</port>
</dependencies>
<node>pwrNode2</node>
</module>

<module>
<name>yarpview</name>
<parameters>--name /PF3DTracker_viewer --x 320 --y 0 --p 50 --compact</parameters>
<node>console</node>
</module>

<connection>
<from>/icubSim/cam/left/rgbImage:o</from>
<to>/pf3dTracker/video:i</to>
<protocol>fast_tcp</protocol>
</connection>
<connection>
<from>/pf3dTracker/video:o</from>
<to>/PF3DTracker_viewer</to>
<protocol>fast_tcp</protocol>
</connection>
<connection>
<from>/pf3dTracker/data:o</from>
<to>/demoRedBall/trackTarget:i</to>
<protocol>fast_tcp</protocol>
</connection>
</application>
12 changes: 12 additions & 0 deletions demoRedBall/gazebo/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
################################################################################
# #
# Copyright (C) 2020 Fondazione Istitito Italiano di Tecnologia (IIT) #
# All Rights Reserved. #
# #
################################################################################

# Install models
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/models DESTINATION share/gazebo)

# Install worlds
install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/worlds DESTINATION share/gazebo)
11 changes: 11 additions & 0 deletions demoRedBall/gazebo/models/red-ball/model.config
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
<?xml version="1.0"?>

<model>
<name>red-ball</name>
<version>1.0</version>
<sdf version="1.7">model.sdf</sdf>

<description>
A red ball.
</description>
</model>
29 changes: 29 additions & 0 deletions demoRedBall/gazebo/models/red-ball/model.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
<?xml version="1.0" ?>

<sdf version="1.7">
<model name="red-ball">

<link name ='root_link'>
<pose>0 0 0 0 0 0</pose>
<inertial>
<mass>700</mass>
</inertial>
<collision name ='collision'>
<geometry>
<sphere><radius>0.020</radius></sphere>
</geometry>
</collision>
<visual name ='visual'>
<geometry>
<sphere><radius>0.025</radius></sphere>
</geometry>
<material>
<ambient>255 0 0 1</ambient>
<diffuse>255 0 0 1</diffuse>
</material>
</visual>
<gravity>false</gravity>
</link>

</model>
</sdf>
49 changes: 49 additions & 0 deletions demoRedBall/gazebo/worlds/grasp-ball-gazebo.sdf
Original file line number Diff line number Diff line change
@@ -0,0 +1,49 @@
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="default">

<include>
<uri>model://sun</uri>
</include>

<state world_name='default'>
<light name='sun'>
<pose frame=''>0 0 10 0 -0 3.14</pose>
</light>
</state>

<include>
<uri>model://ground_plane</uri>
</include>

<model name="iCub">
<include>
<uri>model://iCubGazeboV2_5_visuomanip</uri>
<pose>0.0 0.0 0.63 0.0 0.0 0.0</pose>
</include>
</model>

<model name="red-ball">
<include>
<uri>model://red-ball</uri>
<plugin name="pose_mover" filename='libdemoRedBall-world.so'/>
</include>
<pose>-0.35 0.0 0.935 0.0 0.0 0.0</pose>
<!--<static>true</static>-->
<!-- workaround, as the static flag does not allow to update the visual position in gazebo -->
<!--<joint name="fixed_to_ground" type="fixed">
<parent>world</parent>
<child>red-ball::root_link</child>
</joint>-->
</model>

<gui fullscreen='0'>
<camera name='user_camera'>
<pose frame=''>-1.3907 -0.0286 0.9762 0 0.15 0</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>

</world>
</sdf>
Loading