Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision

Target

Select target project
  • ceti/ros/ccf/ccf_immersive_sorting
1 result
Select Git revision
Show changes
Commits on Source (41)
Hide whitespace changes
Inline Side-by-side
Showing
with 2572 additions and 59 deletions
.clang-format 0 → 100644
View file @ ac8a88e7
---
BasedOnStyle: Google
AccessModifierOffset: -2
ConstructorInitializerIndentWidth: 2
AlignEscapedNewlinesLeft: false
AlignTrailingComments: true
AllowAllParametersOfDeclarationOnNextLine: false
AllowShortIfStatementsOnASingleLine: false
AllowShortLoopsOnASingleLine: false
AllowShortFunctionsOnASingleLine: None
AlwaysBreakTemplateDeclarations: true
AlwaysBreakBeforeMultilineStrings: true
BreakBeforeBinaryOperators: false
BreakBeforeTernaryOperators: false
BreakConstructorInitializersBeforeComma: true
BinPackParameters: true
ColumnLimit: 120
ConstructorInitializerAllOnOneLineOrOnePerLine: true
DerivePointerBinding: false
PointerBindsToType: true
ExperimentalAutoDetectBinPacking: false
IndentCaseLabels: true
MaxEmptyLinesToKeep: 1
NamespaceIndentation: None
ObjCSpaceBeforeProtocolList: true
PenaltyBreakBeforeFirstCallParameter: 19
PenaltyBreakComment: 60
PenaltyBreakString: 1
PenaltyBreakFirstLessLess: 1000
PenaltyExcessCharacter: 1000
PenaltyReturnTypeOnItsOwnLine: 90
SpacesBeforeTrailingComments: 2
Cpp11BracedListStyle: false
Standard: Auto
IndentWidth: 2
TabWidth: 2
UseTab: Never
IndentFunctionDeclarationAfterType: false
SpacesInParentheses: false
SpacesInAngles: false
SpaceInEmptyParentheses: false
SpacesInCStyleCastParentheses: false
SpaceAfterControlStatementKeyword: true
SpaceBeforeAssignmentOperators: true
ContinuationIndentWidth: 4
SortIncludes: false
SpaceAfterCStyleCast: false
# Configure each individual brace in BraceWrapping
BreakBeforeBraces: Custom
# Control of individual brace wrapping cases
BraceWrapping: {
AfterClass: 'true'
AfterControlStatement: 'true'
AfterEnum : 'true'
AfterFunction : 'true'
AfterNamespace : 'true'
AfterStruct : 'true'
AfterUnion : 'true'
BeforeCatch : 'true'
BeforeElse : 'true'
IndentBraces : 'false'
}
...
CMakeLists.txt
View file @ ac8a88e7
......@@ -135,7 +135,8 @@ include_directories(
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
add_executable(${PROJECT_NAME}_dummy_selection_provider src/dummy_selection_provider.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} )
add_executable(${PROJECT_NAME}_dummy_sorting_controller src/dummy_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
add_executable(${PROJECT_NAME}_main_controller src/main_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS} src/cell.cpp src/action.cpp src/task.cpp src/cell_controller.cpp include/cell_controller.h)
add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_controller.cpp ${PROTO_SRCS} ${PROTO_HDRS})
......@@ -145,18 +146,23 @@ add_executable(${PROJECT_NAME}_moveit_sorting_controller src/moveit_sorting_cont
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
set_target_properties(${PROJECT_NAME}_dummy_selection_provider PROPERTIES OUTPUT_NAME dummy_selection_provider PREFIX "")
set_target_properties(${PROJECT_NAME}_dummy_sorting_controller PROPERTIES OUTPUT_NAME dummy_sorting_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_main_controller PROPERTIES OUTPUT_NAME main_controller PREFIX "")
set_target_properties(${PROJECT_NAME}_moveit_sorting_controller PROPERTIES OUTPUT_NAME moveit_sorting_controller PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
add_dependencies(${PROJECT_NAME}_dummy_selection_provider ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_dummy_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_main_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_moveit_sorting_controller ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(${PROJECT_NAME}_dummy_sorting_controller
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_main_controller
${catkin_LIBRARIES}
)
target_link_libraries(${PROJECT_NAME}_moveit_sorting_controller
${catkin_LIBRARIES}
)
......
config/config_scene.yaml deleted 100644 → 0
View file @ 70fc8a49
{ 'objects': [
{ 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.46,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
{ 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.25,'y': -0.2,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'objectRed3','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
{ 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectGreen3','type': 'BOX','pos': { 'x': 0.1,'y': -0.3,'z': 0.819 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'objectBlue2','type': 'BOX','pos': { 'x': -0.3,'y': -0.3,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'objectBlue3','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_ads-table.json 0 → 100644
View file @ ac8a88e7
{
"objects": [
{
"id": "leg_1",
"pos": {
"x": 0.426,
"y": 0.83,
"z": 0.327
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.634
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_2",
"pos": {
"x": -0.426,
"y": -0.83,
"z": 0.327
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.634
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_3",
"pos": {
"x": -0.426,
"y": 0.83,
"z": 0.327
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.634
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_4",
"pos": {
"x": 0.426,
"y": -0.83,
"z": 0.327
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.634
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table-top",
"pos": {
"z": 0.668
},
"size": {
"length": 0.892,
"width": 1.7,
"height": 0.068
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "front_bar",
"pos": {
"z": 0.257,
"x": 0.426
},
"size": {
"length": 0.04,
"width": 1.62,
"height": 0.04
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "back_bar",
"pos": {
"z": 0.257,
"x": -0.426
},
"size": {
"length": 0.04,
"width": 1.62,
"height": 0.04
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "left_bar",
"pos": {
"z": 0.257,
"y": -0.83
},
"size": {
"length": 0.892,
"width": 0.04,
"height": 0.04
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "right_bar",
"pos": {
"z": 0.257,
"y": 0.83
},
"size": {
"length": 0.892,
"width": 0.04,
"height": 0.04
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "binBlue",
"type": "BIN",
"pos": {
"x": 0.19,
"y": -0.6,
"z": 0.772
},
"size": {
"length": 0.2,
"width": 0.28,
"height": 0.14
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "binRed",
"type": "BIN",
"pos": {
"x": -0.23,
"y": -0.4,
"z": 0.772
},
"size": {
"length": 0.2,
"width": 0.28,
"height": 0.14
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "binGreen",
"type": "BIN",
"pos": {
"x": -0.02,
"y": -0.5,
"z": 0.772
},
"size": {
"length": 0.2,
"width": 0.28,
"height": 0.14
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "bigYellow",
"type": "BOX",
"pos": {
"x": 0.15,
"y": 0.4,
"z": 0.7331
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.0622
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "bigRed",
"type": "BOX",
"pos": {
"x": 0.3,
"y": 0,
"z": 0.7427
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "bigGreen",
"type": "BOX",
"pos": {
"x": 0.15,
"y": 0,
"z": 0.7475
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "bigBlue",
"type": "BOX",
"pos": {
"x": -0.05,
"y": 0.4,
"z": 0.7475
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "smallYellow",
"type": "BOX",
"pos": {
"x": 0.15,
"y": -0.2,
"z": 0.7427
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "smallBlue",
"type": "BOX",
"pos": {
"x": 0.15,
"y": 0.2,
"z": 0.7427
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "arm",
"type": "ARM",
"pos": {
"z": 0.702,
"x": -0.22
},
"size": {},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1,
"b": 1
}
}
]
}
config/config_scene_ads-table.yaml 0 → 100644
View file @ ac8a88e7
# create json file using
# yq eval -o=json config_scene_ads-table.yaml > config_scene_ads-table.json
{ 'objects': [
# table
# height: 63.4 (legs) + 6.8 (top) = 70.2 cm total
# legs width/depth/height 4x4x63.4
# table-top width/depth/height 170x89.2x6.8 (169.9 too fuzzy to model)
# panels are 50*70, with 0.5 cm distance to the table-top
# height of both is 12 + 76 + .5 = 88.5
# their centre is at 40 + 0.25 + 25 = 65.25
{ 'id': 'leg_1','pos': { 'x': 0.426,'y': 0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'leg_2','pos': { 'x': -0.426,'y': -0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'leg_3','pos': { 'x': -0.426,'y': 0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'leg_4','pos': { 'x': 0.426,'y': -0.83,'z': 0.327 },'size': { 'length': 0.04,'width': 0.04,'height': 0.634 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table-top', 'pos': { 'z': 0.668 },'size': { 'length': 0.892,'width': 1.7,'height': 0.068 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'front_bar', 'pos': { 'z': 0.257, 'x': 0.426 },'size': { 'length': 0.04,'width': 1.62,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'back_bar', 'pos': { 'z': 0.257, 'x': -0.426 },'size': { 'length': 0.04,'width': 1.62,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'left_bar', 'pos': { 'z': 0.257, 'y': -0.83 },'size': { 'length': 0.892,'width': 0.04,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'right_bar', 'pos': { 'z': 0.257, 'y': 0.83 },'size': { 'length': 0.892,'width': 0.04,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# 12 cm from side edges of the table:
# x: edge -45 + halfsize 10 + distance 12 = -23 // second + distance 1 + fullsize 20 = -2 // third = 19
# y: edge -85 + halfsize 14 + distance 12 = -59 // second + distance 1 + fullsize 28 = -30
{ 'id': 'binBlue', 'type': 'BIN','pos': { 'x': 0.19,'y': -0.60,'z': 0.772 },'size': { 'length': 0.20,'width': 0.28,'height': 0.14 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed', 'type': 'BIN','pos': { 'x': -0.23,'y': -0.40,'z': 0.772 },'size': { 'length': 0.20,'width': 0.28,'height': 0.14 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'binGreen','type': 'BIN','pos': { 'x': -0.02,'y': -0.50,'z': 0.772 },'size': { 'length': 0.20,'width': 0.28,'height': 0.14 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
# for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+7160)/10000, (i*96+46)/10000));
{ 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.15,'y': 0.400000,'z': 0.733100 },'size': { 'length': .0318, 'width': .0636,'height': 0.062200 },'orientation': { 'w': 1 },'color': { r: 1, g: 1 } },
{ 'id': 'bigRed', 'type': 'BOX','pos': { 'x': 0.30,'y': 0.000000,'z': 0.742700 },'size': { 'length': .0318, 'width': .0636,'height': 0.081400 },'orientation': { 'w': 1 },'color': { r: 1 } },
{ 'id': 'bigGreen', 'type': 'BOX','pos': { 'x': 0.15,'y': 0.000000,'z': 0.747500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { g: 1 } },
{ 'id': 'bigBlue', 'type': 'BOX','pos': { 'x': -0.05,'y': 0.400000,'z': 0.747500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { b: 1 } },
# for i in range(3,10): print("{ 'id': 'object2x2x%.1f','type': 'BOX','pos': { 'x': 0.5,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0318,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+7020)/10000, (i*96+46)/10000));
{ 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.15,'y': -0.200000,'z': 0.742700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { r: 1, g: 1 } },
{ 'id': 'smallBlue', 'type': 'BOX','pos': { 'x': 0.15,'y': 0.200000,'z': 0.742700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { b: 1 } },
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.702 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_ads.yaml deleted 100644 → 0
View file @ 70fc8a49
{ 'objects': [
{ 'id': 'tablePillar1','pos': { 'x': 0.625,'y': 0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar2','pos': { 'x': -0.215,'y': -0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar3','pos': { 'x': -0.215,'y': 0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar4','pos': { 'x': 0.625,'y': -0.82,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'table','pos': { 'x' : 0.205 ,'z': 0.7 },'size': { 'length': 0.9,'width': 1.7,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'binBlue','type': 'BIN','pos': { 'x': 0.34,'y': -0.7,'z': 0.8325 },'size': { 'length': 0.2,'width': 0.3,'height': 0.15 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': -0.7,'z': 0.8325 },'size': { 'length': 0.2,'width': 0.3,'height': 0.15 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
{ 'id': 'objectRed2','type': 'BOX','pos': { 'x': 0.0, 'y' : 0.5,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
{ 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.1,'y': 0.6,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectGreen2','type': 'BOX','pos': { 'x': 0.45,'y': 0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'objectBlue2','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_ceti-table-1.json 0 → 100644
View file @ ac8a88e7
{
"objects": [
{
"id": "wheel_1",
"pos": {
"x": 0.28,
"y": 0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_2",
"pos": {
"x": -0.28,
"y": -0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_3",
"pos": {
"x": -0.28,
"y": 0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_4",
"pos": {
"x": 0.28,
"y": -0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_front",
"pos": {
"x": 0.345,
"z": 0.5
},
"size": {
"length": 0.005,
"width": 0.69,
"height": 0.76
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_back",
"pos": {
"x": -0.345,
"z": 0.45
},
"size": {
"length": 0.005,
"width": 0.69,
"height": 0.66
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_left",
"pos": {
"y": 0.345,
"z": 0.5
},
"size": {
"length": 0.69,
"width": 0.005,
"height": 0.76
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_right",
"pos": {
"y": -0.345,
"z": 0.5
},
"size": {
"length": 0.69,
"width": 0.005,
"height": 0.76
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table-top",
"pos": {
"z": 0.885
},
"size": {
"length": 0.8,
"width": 0.8,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "left_panel",
"pos": {
"z": 0.885,
"y": -0.6525
},
"size": {
"length": 0.7,
"width": 0.5,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "right_panel",
"pos": {
"z": 0.885,
"y": 0.6525
},
"size": {
"length": 0.7,
"width": 0.5,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "front_panel",
"pos": {
"z": 0.885,
"x": 0.6525
},
"size": {
"length": 0.5,
"width": 0.7,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "binBlue",
"type": "BIN",
"pos": {
"x": -0.255,
"y": 0.5475,
"z": 0.9675
},
"size": {
"length": 0.19,
"width": 0.285,
"height": 0.155
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "binRed",
"type": "BIN",
"pos": {
"x": -0.055,
"y": 0.5475,
"z": 0.9675
},
"size": {
"length": 0.19,
"width": 0.285,
"height": 0.155
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "binGreen",
"type": "BIN",
"pos": {
"x": 0.145,
"y": 0.5475,
"z": 0.9675
},
"size": {
"length": 0.19,
"width": 0.285,
"height": 0.155
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "bigYellow",
"type": "BOX",
"pos": {
"x": 0.2,
"y": 0.05,
"z": 0.9403
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.1006
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "bigGreen",
"type": "BOX",
"pos": {
"x": 0.3,
"y": 0.3,
"z": 0.9355
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "bigBlue",
"type": "BOX",
"pos": {
"x": 0.1,
"y": -0.25,
"z": 0.9355
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "smallBlue",
"type": "BOX",
"pos": {
"x": 0.1,
"y": 0.2,
"z": 0.9307
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "smallYellow",
"type": "BOX",
"pos": {
"x": 0.3,
"y": -0.2,
"z": 0.9307
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "smallRed",
"type": "BOX",
"pos": {
"x": 0.15,
"y": -0.15,
"z": 0.9307
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "arm",
"type": "ARM",
"pos": {
"z": 0.89,
"x": -0.22
},
"size": {},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1,
"b": 1
}
}
]
}
config/config_scene_ceti-table-1.yaml 0 → 100644
View file @ ac8a88e7
# create json file using
# yq eval -o=json config_scene_ceti-table-1.yaml > config_scene_ceti-table-1.json
{ 'objects': [
# table
# height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total
# wheels width/depth/height 12*12*12
# body width/depth/height 69*69*76
# table-top width/depth/height 80*80*1
# panels are 50*70, with 0.5 cm distance to the table-top
# height of both is 12 + 76 + .5 = 88.5
# their centre is at 40 + 0.25 + 25 = 65.25
{ 'id': 'wheel_1','pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_2','pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_3','pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_4','pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# a filled table and a backplate going all the way up is not possible, because of the oversized collision model of the robot
# { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_front','pos': { 'x': .345, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_back', 'pos': { 'x': -.345, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_left', 'pos': { 'y': .345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_right','pos': { 'y': -.345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table-top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'left_panel', 'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'right_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# bins width/depth/height 28.5*19*15.5
# their height is 89 + 7.75 = 0.9675 m
# the width is 31.8 or 63.6
# if they stand on a panel edge, they stand at 40 + 0.5 + 14.25 = 54.75
# their x location is N*19 + 9.5 - 35 = N*20 - 25.5 = -25.5 and -5.5 and 14.5 if they have 1 cm distance
{ 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.255,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed', 'type': 'BIN','pos': { 'x': -0.055,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'binGreen','type': 'BIN','pos': { 'x': 0.145,'y': 0.5475,'z': 0.9675 },'size': { 'length': 0.19,'width': 0.285,'height': 0.155 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.20,'y': 0.05,'z': 0.940300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1 } },
{ 'id': 'bigGreen','type': 'BOX','pos': { 'x': 0.30,'y': 0.30,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'bigBlue','type': 'BOX','pos': { 'x': 0.10,'y': -0.25,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'smallBlue','type': 'BOX','pos': { 'x': 0.10,'y': 0.20,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.30,'y': -0.20,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1 } },
{ 'id': 'smallRed','type': 'BOX','pos': { 'x': 0.15,'y': -0.15,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
# heights of objects (including pins):
# formula: half-height * 9.6 + 4.6
# formula (python)
# for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+8900)/10000, (i*96+46)/10000));
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_ceti-table-2.json 0 → 100644
View file @ ac8a88e7
{
"objects": [
{
"id": "wheel_1",
"pos": {
"x": 0.28,
"y": 0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_2",
"pos": {
"x": -0.28,
"y": -0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_3",
"pos": {
"x": -0.28,
"y": 0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_4",
"pos": {
"x": 0.28,
"y": -0.28,
"z": 0.06
},
"size": {
"length": 0.12,
"width": 0.12,
"height": 0.12
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_front",
"pos": {
"x": 0.345,
"z": 0.5
},
"size": {
"length": 0.005,
"width": 0.69,
"height": 0.76
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_back",
"pos": {
"x": -0.345,
"z": 0.45
},
"size": {
"length": 0.005,
"width": 0.69,
"height": 0.66
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_left",
"pos": {
"y": 0.345,
"z": 0.5
},
"size": {
"length": 0.69,
"width": 0.005,
"height": 0.76
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_body_right",
"pos": {
"y": -0.345,
"z": 0.5
},
"size": {
"length": 0.69,
"width": 0.005,
"height": 0.76
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table-top",
"pos": {
"z": 0.885
},
"size": {
"length": 0.8,
"width": 0.8,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "left_panel",
"pos": {
"z": 0.885,
"y": -0.6525
},
"size": {
"length": 0.7,
"width": 0.5,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "right_panel",
"pos": {
"z": 0.885,
"y": 0.6525
},
"size": {
"length": 0.7,
"width": 0.5,
"height": 0.01
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "binBlue",
"type": "BIN",
"pos": {
"x": -0.23,
"y": 0.67,
"z": 0.9425
},
"size": {
"length": 0.24,
"width": 0.17,
"height": 0.105
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "binRed",
"type": "BIN",
"pos": {
"x": -0.23,
"y": 0.49,
"z": 0.9425
},
"size": {
"length": 0.24,
"width": 0.17,
"height": 0.105
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "binGreen",
"type": "BIN",
"pos": {
"x": -0.23,
"y": -0.49,
"z": 0.9425
},
"size": {
"length": 0.24,
"width": 0.17,
"height": 0.105
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "bigYellow",
"type": "BOX",
"pos": {
"x": 0.1,
"y": -0.25,
"z": 0.9403
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.1006
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "bigGreen",
"type": "BOX",
"pos": {
"x": 0.1,
"y": 0.3,
"z": 0.9403
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.1006
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "bigBlue",
"type": "BOX",
"pos": {
"x": 0.2,
"y": -0.1,
"z": 0.9355
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"z": 0.7071068,
"w": 0.7071068
},
"color": {
"b": 1
}
},
{
"id": "bigRed",
"type": "BOX",
"pos": {
"x": 0.15,
"y": 0.1,
"z": 0.9355
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"z": 0.7071068,
"w": 0.7071068
},
"color": {
"r": 1
}
},
{
"id": "smallRed",
"type": "BOX",
"pos": {
"x": 0.3,
"y": -0.25,
"z": 0.9307
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "smallYellow",
"type": "BOX",
"pos": {
"x": 0.25,
"y": 0.25,
"z": 0.9211
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0622
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "arm",
"type": "ARM",
"pos": {
"z": 0.89,
"x": -0.22
},
"size": {},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1,
"b": 1
}
}
]
}
config/config_scene_ceti-table-2.yaml 0 → 100644
View file @ ac8a88e7
# create json file using
# yq eval -o=json config_scene_ceti-table-2.yaml > config_scene_ceti-table-2.json
{ 'objects': [
# table
# height: 12 cm wheels, 76 cm body, 1 cm table-top = 89 cm total
# wheels width/depth/height 12*12*12
# body width/depth/height 69*69*76
# table-top width/depth/height 80*80*1
# panels are 50*70, with 0.5 cm distance to the table-top
# height of both is 12 + 76 + .5 = 88.5
# their centre is at 40 + 0.25 + 25 = 65.25
{ 'id': 'wheel_1','pos': { 'x': 0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_2','pos': { 'x': -0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_3','pos': { 'x': -0.28,'y': 0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_4','pos': { 'x': 0.28,'y': -0.28,'z': 0.06 },'size': { 'length': 0.12,'width': 0.12,'height': 0.12 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# a filled table and a backplate going all the way up is not possible, because of the oversized collision model of the robot
# { 'id': 'table_body','pos': { 'z': 0.50 },'size': { 'length': .69,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_front','pos': { 'x': .345, 'z': 0.50 },'size': { 'length': 0.005,'width': 0.69,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_back', 'pos': { 'x': -.345, 'z': 0.45 },'size': { 'length': 0.005,'width': 0.69,'height': 0.66 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_left', 'pos': { 'y': .345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_body_right','pos': { 'y': -.345, 'z': 0.50 },'size': { 'length': 0.69,'width': 0.005,'height': 0.76 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table-top', 'pos': { 'z': 0.885 },'size': { 'length': 0.8,'width': 0.8,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'left_panel', 'pos': { 'z': 0.885, 'y': -0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'right_panel', 'pos': { 'z': 0.885, 'y': 0.6525 },'size': { 'length': 0.7,'width': 0.5,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# { 'id': 'front_panel', 'pos': { 'z': 0.885, 'x': 0.6525 },'size': { 'length': 0.5,'width': 0.7,'height': 0.01 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# bins width/depth/height
# their height is .89 + 0.0525 = 0.9425 m
# the width is 31.8 or 63.6
# if they stand on a panel edge, they stand at 40 + 0.5 + 8.5 = 49 and 49 + 1 + 17 = 67
# their x location is 12 - 35 = -23
{ 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.23,'y': 0.67,'z': 0.9425 },'size': { 'length': 0.24,'width': 0.17,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed', 'type': 'BIN','pos': { 'x': -0.23,'y': 0.49,'z': 0.9425 },'size': { 'length': 0.24,'width': 0.17,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'binGreen','type': 'BIN','pos': { 'x': -0.23,'y': -0.49,'z': 0.9425 },'size': { 'length': 0.24,'width': 0.17,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.10,'y': -0.25,'z': 0.940300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1 } },
{ 'id': 'bigGreen' ,'type': 'BOX','pos': { 'x': 0.10,'y': 0.30,'z': 0.940300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { 'g': 1 } },
{ 'id': 'bigBlue' ,'type': 'BOX','pos': { 'x': 0.20,'y': -0.10,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'z': 0.7071068, 'w':0.7071068 },'color': { 'b': 1 } },
{ 'id': 'bigRed' ,'type': 'BOX','pos': { 'x': 0.15,'y': 0.10,'z': 0.935500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'z': 0.7071068, 'w':0.7071068 },'color': { 'r': 1 } },
{ 'id': 'smallRed','type': 'BOX','pos': { 'x': 0.30,'y': -0.25,'z': 0.930700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.25,'y': 0.25,'z': 0.921100 },'size': { 'length': .0318, 'width': .0318,'height': 0.062200 },'orientation': { 'w': 1 },'color': { 'r': 1, 'g': 1 } },
# heights of objects (including pins):
# formula: half-height * 9.6 + 4.6
# formula (python)
# for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+8900)/10000, (i*96+46)/10000));
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.89 , 'x': -0.22 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_st-table.json 0 → 100644
View file @ ac8a88e7
{
"objects": [
{
"id": "wheel_1",
"pos": {
"x": -0.148,
"y": 0.67,
"z": 0.0655
},
"size": {
"length": 0.18,
"width": 0.18,
"height": 0.131
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_2",
"pos": {
"x": -0.148,
"y": -0.67,
"z": 0.0655
},
"size": {
"length": 0.18,
"width": 0.18,
"height": 0.131
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_3",
"pos": {
"x": 0.554,
"y": 0.67,
"z": 0.0655
},
"size": {
"length": 0.18,
"width": 0.18,
"height": 0.131
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "wheel_4",
"pos": {
"x": 0.554,
"y": -0.67,
"z": 0.0655
},
"size": {
"length": 0.18,
"width": 0.18,
"height": 0.131
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_1",
"pos": {
"x": -0.225,
"y": 0.83,
"z": 0.39025
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.5185
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_2",
"pos": {
"x": -0.225,
"y": -0.83,
"z": 0.39025
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.5185
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_3",
"pos": {
"x": 0.633,
"y": 0.83,
"z": 0.39025
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.5185
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "leg_4",
"pos": {
"x": 0.633,
"y": -0.83,
"z": 0.39025
},
"size": {
"length": 0.04,
"width": 0.04,
"height": 0.5185
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_lower_shelf",
"pos": {
"x": 0.203,
"z": 0.151
},
"size": {
"length": 0.9,
"width": 1.7,
"height": 0.04
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_mid_shelf",
"pos": {
"x": 0.203,
"z": 0.333
},
"size": {
"length": 0.9,
"width": 1.7,
"height": 0.04
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "table_top",
"pos": {
"x": 0.203,
"z": 0.68275
},
"size": {
"length": 0.9,
"width": 1.7,
"height": 0.0665
},
"orientation": {
"w": 1
},
"color": {
"r": 0.15,
"g": 0.15,
"b": 0.15
}
},
{
"id": "binGreen",
"type": "BIN",
"pos": {
"x": -0,
"y": -0.47,
"z": 0.7685
},
"size": {
"length": 0.17,
"width": 0.24,
"height": 0.105
},
"orientation": {
"w": 1
},
"color": {
"g": 0.8
}
},
{
"id": "binRed",
"type": "BIN",
"pos": {
"x": 0.18,
"y": -0.47,
"z": 0.7685
},
"size": {
"length": 0.17,
"width": 0.24,
"height": 0.105
},
"orientation": {
"w": 1
},
"color": {
"r": 0.8
}
},
{
"id": "binBlue",
"type": "BIN",
"pos": {
"x": 0.36,
"y": -0.47,
"z": 0.7685
},
"size": {
"length": 0.17,
"width": 0.24,
"height": 0.105
},
"orientation": {
"w": 1
},
"color": {
"b": 0.8
}
},
{
"id": "bigRed",
"type": "BOX",
"pos": {
"x": 0.3,
"y": 0.3,
"z": 0.7615
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.091
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "bigYellow",
"type": "BOX",
"pos": {
"x": 0.3,
"y": 0,
"z": 0.7663
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.1006
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "bigBlue",
"type": "BOX",
"pos": {
"x": 0.3,
"y": 0.1,
"z": 0.7663
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.1006
},
"orientation": {
"w": 1
},
"color": {
"b": 1
}
},
{
"id": "bigGreen",
"type": "BOX",
"pos": {
"x": 0.3,
"y": 0.2,
"z": 0.7663
},
"size": {
"length": 0.0318,
"width": 0.0636,
"height": 0.1006
},
"orientation": {
"w": 1
},
"color": {
"g": 1
}
},
{
"id": "smallRed",
"type": "BOX",
"pos": {
"x": 0.5,
"y": -0.1,
"z": 0.7567
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1
}
},
{
"id": "smallYellow",
"type": "BOX",
"pos": {
"x": 0.5,
"y": 0,
"z": 0.7567
},
"size": {
"length": 0.0318,
"width": 0.0318,
"height": 0.0814
},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1
}
},
{
"id": "arm",
"type": "ARM",
"pos": {
"z": 0.716
},
"size": {},
"orientation": {
"w": 1
},
"color": {
"r": 1,
"g": 1,
"b": 1
}
}
]
}
config/config_scene_st-table.yaml 0 → 100644
View file @ ac8a88e7
# create json file using
# yq eval -o=json config_scene_st-table.yaml > config_scene_st-table.json
{ 'objects': [
# table height = 13.1 + 58.5 = 71.6
{ 'id': 'wheel_1','pos': { 'x': -0.148,'y': 0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_2','pos': { 'x': -0.148,'y': -0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_3','pos': { 'x': 0.554,'y': 0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'wheel_4','pos': { 'x': 0.554,'y': -0.67,'z': 0.0655 },'size': { 'length': 0.18,'width': 0.18,'height': 0.131 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# leg z:
{ 'id': 'leg_1','pos': { 'x': -0.225,'y': 0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'leg_2','pos': { 'x': -0.225,'y': -0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'leg_3','pos': { 'x': 0.633,'y': 0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'leg_4','pos': { 'x': 0.633,'y': -0.83,'z': 0.39025 },'size': { 'length': 0.04,'width': 0.04,'height': 0.5185 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# top z pos = 71.6 - (6.65)/2 = 71.6 - 3.325 = 68.275
{ 'id': 'table_lower_shelf','pos': { 'x': 0.203, 'z': 0.151 },'size': { 'length': 0.9,'width': 1.7,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_mid_shelf','pos': { 'x': 0.203, 'z': 0.333 },'size': { 'length': 0.9,'width': 1.7,'height': 0.04 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
{ 'id': 'table_top','pos': { 'x': 0.203, 'z': 0.68275 },'size': { 'length': 0.9,'width': 1.7,'height': 0.0665 },'orientation': { 'w': 1 },'color': { 'r': 0.15,'g': 0.15,'b': 0.15 } },
# zpos = 71.6 + 5.25 = 76.85
# most behind outer corner at: 85-47-12 = 20cm
{ 'id': 'binGreen', 'type': 'BIN','pos': { 'x': -0.00,'y': -0.47,'z': 0.7685 },'size': { 'length': 0.17,'width': 0.24,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'g': 0.8 } },
{ 'id': 'binRed', 'type': 'BIN','pos': { 'x': 0.18,'y': -0.47,'z': 0.7685 },'size': { 'length': 0.17,'width': 0.24,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'r': 0.8 } },
{ 'id': 'binBlue', 'type': 'BIN','pos': { 'x': 0.36,'y': -0.47,'z': 0.7685 },'size': { 'length': 0.17,'width': 0.24,'height': 0.105 },'orientation': { 'w': 1 },'color': { 'b': 0.8 } },
# for i in range(3,10): print("{ 'id': 'object4x2x%.1f','type': 'BOX','pos': { 'x': 0.3,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0636,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+7160)/10000, (i*96+46)/10000));
{ 'id': 'bigRed','type': 'BOX','pos': { 'x': 0.3,'y': 0.300000,'z': 0.761500 },'size': { 'length': .0318, 'width': .0636,'height': 0.091000 },'orientation': { 'w': 1 },'color': { r: 1 } },
{ 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.3,'y': 0.000000,'z': 0.766300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { r: 1, g: 1 } },
{ 'id': 'bigBlue','type': 'BOX','pos': { 'x': 0.3,'y': 0.100000,'z': 0.766300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { b: 1 } },
{ 'id': 'bigGreen','type': 'BOX','pos': { 'x': 0.3,'y': 0.200000,'z': 0.766300 },'size': { 'length': .0318, 'width': .0636,'height': 0.100600 },'orientation': { 'w': 1 },'color': { g: 1 } },
# for i in range(3,10): print("{ 'id': 'object2x2x%.1f','type': 'BOX','pos': { 'x': 0.5,'y': %f,'z': %f },'size': { 'length': .0318, 'width': .0318,'height': %f },'orientation': { 'w': 1 },'color': {} }," % (i/2, (i-6)*.1, (i*48+23+7160)/10000, (i*96+46)/10000));
{ 'id': 'smallRed','type': 'BOX','pos': { 'x': 0.5,'y': -0.100000,'z': 0.756700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { r: 1 } },
{ 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.5,'y': 0.00000,'z': 0.756700 },'size': { 'length': .0318, 'width': .0318,'height': 0.081400 },'orientation': { 'w': 1 },'color': { r: 1, g: 1 } },
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.716 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_st.yaml deleted 100644 → 0
View file @ 70fc8a49
{ 'objects': [
{ 'id': 'tablePillar1','pos': { 'x': 0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar2','pos': { 'x': -0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar3','pos': { 'x': -0.77,'y': 0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'tablePillar4','pos': { 'x': 0.77,'y': -0.77,'z': 0.325 },'size': { 'length': 0.06,'width': 0.06,'height': 0.65 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'table','pos': { 'z': 0.7 },'size': { 'length': 1.6,'width': 1.6,'height': 0.1 },'orientation': { 'w': 1 },'color': { 'r': 255,'g': 222,'b': 173 } },
{ 'id': 'binBlue','type': 'BIN','pos': { 'x': -0.34,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'b': 1 } },
{ 'id': 'binRed','type': 'BIN','pos': { 'x': 0.06,'y': 0.49,'z': 0.8325 },'size': { 'length': 0.21,'width': 0.3,'height': 0.165 },'orientation': { 'w': 1 },'color': { 'r': 1 } },
{ 'id': 'objectRed1','type': 'BOX','pos': { 'x': 0.5,'y': -0.1,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
{ 'id': 'objectRed2','type': 'BOX','pos': { 'x': -0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'r': 1 } },
{ 'id': 'objectGreen1','type': 'BOX','pos': { 'x': 0.45,'y': -0.3,'z': 0.8105 },'size': {'length':0.031,'width':0.062,'height':0.121},'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectGreen2','type': 'BOX','pos': { 'x': -0.45,'y': -0.2,'z': 0.8105 },'size': {'length':0.031,'width':0.031,'height':0.138},'orientation': { 'z': 1,'w': 0.92388 },'color': { 'g': 1 } },
{ 'id': 'objectBlue1','type': 'BOX','pos': { 'x': 0.25,'y': -0.4,'z': 0.8105 },'size': { 'length': 0.031,'width': 0.062,'height': 0.121 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'objectBlue2','type': 'BOX','pos': { 'x': 0.4,'z': 0.819 },'size': { 'length': 0.031,'width': 0.031,'height': 0.138 },'orientation': { 'z': 0.382683,'w': 0.92388 },'color': { 'b': 1 } },
{ 'id': 'arm','type': 'ARM','pos': { 'z': 0.75 },'size': { },'orientation': { 'w': 1 },'color': { 'r': 1.00,'g': 1.00,'b': 1.00 } }
] }
config/config_scene_virtual-table.json 0 → 100644
View file @ ac8a88e7
{
"objects": [
{
"id": "table",
"pos": {
"z": 0.25
},
"size": {
"length": 1,
"width": 2,
"height": 0.5
},
"orientation": {
"w": 1
},
"color": {
"r": 0.3,
"g": 0.3,
"b": 0.3
}
},
{
"id": "binBlue",
"type": "BIN",
"pos": {
"x": -0.35,
"y": -0.6,
"z": 0.65
},
"size": {
"length": 0.2,
"width": 0.4,
"height": 0.3
},
"orientation": {
"w": 1
},
"color": {
"b": 0.5
}
},
{
"id": "binGreen",
"type": "BIN",
"pos": {
"x": -0.35,
"y": 0,
"z": 0.65
},
"size": {
"length": 0.2,
"width": 0.4,
"height": 0.3
},
"orientation": {
"w": 1
},
"color": {
"g": 0.5
}
},
{
"id": "binRed",
"type": "BIN",
"pos": {
"x": -0.35,
"y": 0.6,
"z": 0.65
},
"size": {
"length": 0.2,
"width": 0.4,
"height": 0.3
},
"orientation": {
"w": 1
},
"color": {
"r": 0.5
}
},
{
"id": "bigBlue",
"type": "BOX",
"pos": {
"x": 0.2,
"y": -0.3,
"z": 0.58
},
"size": {
"length": 0.1,
"width": 0.16,
"height": 0.16
},
"orientation": {
"w": 1
},
"color": {
"b": 0.5
}
},
{
"id": "bigGreen",
"type": "BOX",
"pos": {
"x": 0.2,
"y": -0.1,
"z": 0.58
},
"size": {
"length": 0.1,
"width": 0.16,
"height": 0.16
},
"orientation": {
"w": 1
},
"color": {
"g": 0.5
}
},
{
"id": "bigRed",
"type": "BOX",
"pos": {
"x": 0.2,
"y": 0.1,
"z": 0.58
},
"size": {
"length": 0.1,
"width": 0.16,
"height": 0.16
},
"orientation": {
"w": 1
},
"color": {
"r": 0.5
}
},
{
"id": "bigYellow",
"type": "BOX",
"pos": {
"x": 0.2,
"y": 0.3,
"z": 0.58
},
"size": {
"length": 0.1,
"width": 0.16,
"height": 0.16
},
"orientation": {
"w": 1
},
"color": {
"r": 0.5,
"g": 0.5
}
},
{
"id": "smallBlue",
"type": "BOX",
"pos": {
"x": 0.4,
"y": -0.2,
"z": 0.54
},
"size": {
"length": 0.1,
"width": 0.08,
"height": 0.08
},
"orientation": {
"w": 1
},
"color": {
"b": 0.5
}
},
{
"id": "smallGreen",
"type": "BOX",
"pos": {
"x": 0.4,
"y": 0,
"z": 0.54
},
"size": {
"length": 0.1,
"width": 0.08,
"height": 0.08
},
"orientation": {
"w": 1
},
"color": {
"g": 0.5
}
},
{
"id": "smallRed",
"type": "BOX",
"pos": {
"x": 0.4,
"y": 0.2,
"z": 0.54
},
"size": {
"length": 0.1,
"width": 0.08,
"height": 0.08
},
"orientation": {
"w": 1
},
"color": {
"r": 0.5
}
},
{
"id": "smallYellow",
"type": "BOX",
"pos": {
"x": 0.4,
"y": 0.4,
"z": 0.54
},
"size": {
"length": 0.1,
"width": 0.08,
"height": 0.08
},
"orientation": {
"w": 1
},
"color": {
"r": 0.5,
"g": 0.5
}
},
{
"id": "arm",
"type": "ARM",
"pos": {},
"size": {},
"orientation": {
"w": 1
},
"color": {}
}
]
}
config/config_scene_virtual-table.yaml 0 → 100644
View file @ ac8a88e7
# create json file using
# yq eval -o=json config_scene_virtual-table.yaml > config_scene_virtual-table.json
{ 'objects': [
# table
# height: 1m
# width: 2m
{ 'id': 'table', 'pos': { 'z': 0.25 },'size': { 'length': 1,'width': 2,'height': .5 },'orientation': { 'w': 1 },'color': { 'r': 0.3,'g': 0.3,'b': 0.3 } },
# idea: all elements are sorted by name
{ 'id': 'binBlue', 'type': 'BIN','pos': { 'x': -0.35,'y': -0.60,'z': 0.65 },'size': { 'length': 0.20,'width': 0.4,'height': 0.3 },'orientation': { 'w': 1 },'color': { 'b': 0.5 } },
{ 'id': 'binGreen', 'type': 'BIN','pos': { 'x': -0.35,'y': 0,'z': 0.65 },'size': { 'length': 0.20,'width': 0.4,'height': 0.3 },'orientation': { 'w': 1 },'color': { 'g': 0.5 } },
{ 'id': 'binRed','type': 'BIN','pos': { 'x': -0.35,'y': 0.60,'z': 0.65 },'size': { 'length': 0.20,'width': 0.4,'height': 0.3 },'orientation': { 'w': 1 },'color': { 'r': 0.5 } },
{ 'id': 'bigBlue', 'type': 'BOX','pos': { 'x': 0.2,'y': -0.300000,'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'b': 0.5 } },
{ 'id': 'bigGreen', 'type': 'BOX','pos': { 'x': 0.2,'y': -0.100000,'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'g': 0.5 } },
{ 'id': 'bigRed', 'type': 'BOX','pos': { 'x': 0.2,'y': 0.100000, 'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'r': 0.5 } },
{ 'id': 'bigYellow','type': 'BOX','pos': { 'x': 0.2,'y': 0.300000, 'z': 0.58 },'size': { 'length': .1, 'width': .16,'height': 0.16 },'orientation': { 'w': 1 },'color': { 'r': 0.5, 'g': 0.5 } },
{ 'id': 'smallBlue', 'type': 'BOX','pos': { 'x': 0.4,'y': -0.200000,'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'b': 0.5 } },
{ 'id': 'smallGreen', 'type': 'BOX','pos': { 'x': 0.4,'y': 0.000000, 'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'g': 0.5 } },
{ 'id': 'smallRed', 'type': 'BOX','pos': { 'x': 0.4,'y': 0.200000, 'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'r': 0.5 } },
{ 'id': 'smallYellow','type': 'BOX','pos': { 'x': 0.4,'y': 0.400000, 'z': 0.54 },'size': { 'length': .1, 'width': .08,'height': 0.08 },'orientation': { 'w': 1 },'color': { 'r': 0.5, 'g': 0.5 } },
{ 'id': 'arm','type': 'ARM','pos': { },'size': { },'orientation': { 'w': 1 },'color': { } }
] }
include/action.h 0 → 100644
View file @ ac8a88e7
#ifndef CCF_IMMERSIVE_SORTING_ACTION_H_
#define CCF_IMMERSIVE_SORTING_ACTION_H_
#include <string>
#include "connector.pb.h"
class Action
{
public:
enum Type
{
UNDEFINED,
PICK_DROP
};
private:
const Type type_;
const std::string source_;
const std::string target_;
public:
bool operator==(const Action& rhs) const
{
return type_ == rhs.type_ && source_ == rhs.source_ && target_ == rhs.target_;
}
bool operator!=(const Action& rhs) const
{
return !(rhs == *this);
}
private:
static constexpr Object::Type requiredSourceType(Type type)
{
switch (type)
{
case PICK_DROP:
return Object::BOX;
case UNDEFINED:
default:
return Object::UNKNOWN;
}
}
static constexpr Object::Type requiredTargetType(Type type)
{
switch (type)
{
case PICK_DROP:
return Object::BIN;
case UNDEFINED:
default:
return Object::UNKNOWN;
}
}
[[nodiscard]] static std::optional<Object> lookupObject(const Scene& scene, const std::string& id)
{
for (Object object : scene.objects())
{
if (object.id() == id)
{
return object;
}
}
return std::nullopt;
}
public:
[[nodiscard]] Type getType() const
{
return type_;
}
[[nodiscard]] const std::string& getSource() const
{
return source_;
}
[[nodiscard]] const std::string& getTarget() const
{
return target_;
}
public:
/**
* Constructor
* @param type
* @param source
* @param target
*/
Action(Type type, std::string source, std::string target)
: type_(type), source_(std::move(source)), target_(std::move(target))
{
}
/**
* an action can be performed in a scene if source and target exist in the scene and have the correct type for the
* action
* @param scene
* @return true if the action can be performed
*/
bool canBePerformed(const Scene& scene)
{
auto s = lookupObject(scene, source_);
auto t = lookupObject(scene, target_);
return s && t && s->type() == requiredSourceType(type_) && t->type() == requiredTargetType(type_);
}
[[nodiscard]] bool isCompleted(const Scene& scene) const
{
switch (type_)
{
case Action::Type::PICK_DROP:
// pick and drop is completed if the dropped element no longer exists
return !lookupObject(scene, source_).has_value();
case Action::Type::UNDEFINED:
// undefined actions are defined to be completed
default:
return true;
}
}
friend std::ostream& operator<<(std::ostream& os, const Action& action);
};
std::ostream& operator<<(std::ostream& os, const Action& action);
#endif // CCF_IMMERSIVE_SORTING_ACTION_H_
include/cell.h 0 → 100644
View file @ ac8a88e7
#ifndef CCF_IMMERSIVE_SORTING_CELL_H_
#define CCF_IMMERSIVE_SORTING_CELL_H_
#include <connector.pb.h>
#include <optional>
#include <string>
struct Cell
{
enum State
{
UNDEFINED,
DISCONNECTED,
IDLE,
WORKING
};
std::string stateString() const {
switch (state) {
case DISCONNECTED:
return "DISCONNECTED";
case IDLE:
return "IDLE";
case WORKING:
return "WORKING";
default:
return "UNDEFINED";
}
}
std::string id;
Scene scene;
State state = UNDEFINED;
std::optional<std::string> processed_object;
Cell() = default;
// one does not copy cells
Cell(const Cell&) = delete;
Cell& operator=(const Cell&) = delete;
};
#endif // CCF_IMMERSIVE_SORTING_CELL_H_
include/cell_controller.h 0 → 100644
View file @ ac8a88e7
#ifndef CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
#define CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
#include <utility>
#include "cell.h"
#include "action.h"
#include "task.h"
#include "ros/ros.h"
class CellController
{
Cell cell_;
std::shared_ptr<Action> current_action_;
std::shared_ptr<Task> current_task_;
std::function<void(const Action &)> action_callback_;
public:
CellController()
{
cell_.state = Cell::DISCONNECTED;
}
void setCellId(const std::string &id)
{
cell_.id = id;
}
void setActionCallback(const std::function<void(const Action &)> &action_callback)
{
action_callback_ = action_callback;
}
const Cell &cell() const
{
return cell_;
}
bool hasCurrentAction() const
{
return current_action_ == nullptr;
}
const Action &getCurrentAction() const
{
return *current_action_;
}
const std::shared_ptr<Task> &getCurrentTask() const
{
return current_task_;
}
void setCurrentTask(const std::shared_ptr<Task> &new_task)
{
if (!currentActionCompleted())
{
ROS_WARN_STREAM(
"Cell " << cell_.id << " got a new task although an action of the previous task is still ongoing.");
}
current_task_.reset(new_task.get());
}
void proceedWithTask()
{
if (currentActionCompleted())
{
auto action = triggerNextAction();
}
}
void updateModel(const std::string &scene_string)
{
ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
if (cell_.state == Cell::DISCONNECTED)
{
cell_.state = Cell::IDLE;
}
cell_.scene.ParseFromString(scene_string);
proceedWithTask();
}
// TODO do we need this method?
void updateModel(const Scene &scene)
{
ROS_INFO_STREAM("Getting a scene update for cell " << cell_.id);
if (cell_.state == Cell::DISCONNECTED)
{
cell_.state = Cell::IDLE;
}
cell_.scene.CopyFrom(scene);
proceedWithTask();
}
/**
* A current action is completed if there is no current action or the current action can no longer be performed
* @return
*/
bool currentActionCompleted()
{
if (current_action_ == nullptr)
{
return true;
}
else if (current_action_->isCompleted(cell_.scene))
{
// the current action is completed, but this was not registered before, so we have to set the cell state
cell_.state = Cell::IDLE;
current_action_ = nullptr;
return true;
}
else
{
return false;
}
}
std::optional<Action> triggerNextAction()
{
// TODO this assumes each action can be performed only once, which holds for pick-and-drop tasks but not in general
for (Action action: current_task_->getActions())
{
if (!action.isCompleted(cell_.scene) && action.canBePerformed(cell_.scene))
{
cell_.state = Cell::WORKING;
ROS_INFO_STREAM("Triggering action " << action);
action_callback_(action);
return action;
}
}
return std::nullopt;
}
};
#endif // CCF_IMMERSIVE_SORTING_CELL_CONTROLLER_H_
include/task.h 0 → 100644
View file @ ac8a88e7
#ifndef CCF_IMMERSIVE_SORTING_TASK_H_
#define CCF_IMMERSIVE_SORTING_TASK_H_
#include <vector>
#include "action.h"
class Task
{
std::vector<Action> actions_;
public:
void addAction(const Action& action)
{
actions_.emplace_back(action);
}
[[nodiscard]] const std::vector<Action>& getActions() const
{
return actions_;
}
};
#endif // CCF_IMMERSIVE_SORTING_TASK_H_
launch/dummy_main.launch → launch/dummy-cell_ads-table.launch
View file @ ac8a88e7
<launch>
<arg name="connection_address" default="tcp://*:6576" />
<arg name="client_controllers" default="[]" />
<arg name="connection_address" default="tcp://*:6576"/>
<node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="my_dummy_sorting_controller" output="screen">
<param name="connection_address" type="string" value="$(arg connection_address)" />
<param name="client_controllers" type="yaml" value="$(arg client_controllers)" />
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene.yaml" />
<node pkg="ccf_immersive_sorting" type="dummy_sorting_controller" name="ads_cell" output="screen">
<param name="connection_address" type="string" value="$(arg connection_address)"/>
<param name="scene" type="string" value="$(find ccf_immersive_sorting)/config/config_scene_ads-table.json"/>
</node>
</launch>