Skip to content

Commit 645df74

Browse files
ichumuhichumuh
authored
Merge pull request #161 from SemRoCo/feature_functions
Feature functions
2 parents 83b833b + 70867e6 commit 645df74

16 files changed

+933
-61
lines changed

‎.github/workflows/ci_standalone.yml

+2
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,8 @@ jobs:
5555
test3: test/test_integration_pr2.py::TestWorldManipulation
5656
test4: test/test_integration_pr2.py::TestManipulability::test_manip1
5757
test5: test/test_integration_pr2.py::TestWeightScaling
58+
test6: test/test_integration_pr2.py::TestFeatureFunctions
59+
test7: test/test_integration_pr2.py::TestEndMotionReason
5860
pr2_part2:
5961
needs: [ build_dependencies ]
6062
uses: ./.github/workflows/reusable_robot_ci.yml

‎launch/giskardpy_pr2_standalone_vrb.launch

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<launch>
2+
3+
<include file="$(find pr2_description)/robots/upload_pr2.launch"/>
4+
5+
<node pkg="giskardpy" type="pr2_standalone_vrb.py" name="giskard" output="screen"/>
6+
7+
</launch>

‎launch/giskardpy_stretch_standalone.launch

+3-3
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,14 @@
11
<launch>
22

3-
<include file="$(find stretch_description)/launch/upload.launch"/>
3+
<param name="robot_description" command="xacro $(find stretch_description)/urdf/stretch_description_dex.xacro" />
44

55
<node pkg="giskardpy" type="stretch_standalone.py" name="giskard" output="screen"/>
66

77
<node pkg="giskardpy" type="interactive_marker.py" name="giskard_interactive_marker" output="screen">
88
<rosparam param="enable_self_collision">False</rosparam>
99
<rosparam param="interactive_marker_chains">
10-
- [map, stretch/link_grasp_center]
11-
- [map, stretch/base_link]
10+
- [map, link_grasp_center]
11+
- [map, base_link]
1212
</rosparam>
1313
</node>
1414

‎launch/giskardpy_stretch_standalone_vrb.launch

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<launch>
2+
3+
<param name="robot_description" command="xacro $(find stretch_description)/urdf/stretch_description_dex.xacro" />
4+
5+
<node pkg="giskardpy" type="stretch_standalone_vrb.py" name="giskard" output="screen"/>
6+
7+
</launch>

‎scripts/iai_robots/pr2/pr2_standalone.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,6 @@
3434
drive_joint_name,
3535
]
3636
),
37-
behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=True),
37+
behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=True, debug_mode=True),
3838
qp_controller_config=QPControllerConfig())
3939
giskard.live()

‎scripts/iai_robots/pr2/pr2_standalone_vrb.py

+40
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
#!/usr/bin/env python
2+
import rospy
3+
4+
from giskardpy.configs.behavior_tree_config import StandAloneBTConfig
5+
from giskardpy.configs.giskard import Giskard
6+
from giskardpy.configs.iai_robots.pr2 import PR2CollisionAvoidance, WorldWithPR2Config
7+
from giskardpy.configs.qp_controller_config import QPControllerConfig, SupportedQPSolver
8+
from giskardpy.configs.robot_interface_config import StandAloneRobotInterfaceConfig
9+
10+
if __name__ == '__main__':
11+
rospy.init_node('giskard')
12+
drive_joint_name = 'brumbrum'
13+
giskard = Giskard(world_config=WorldWithPR2Config(drive_joint_name=drive_joint_name),
14+
collision_avoidance_config=PR2CollisionAvoidance(drive_joint_name=drive_joint_name),
15+
robot_interface_config=StandAloneRobotInterfaceConfig(
16+
[
17+
'torso_lift_joint',
18+
'head_pan_joint',
19+
'head_tilt_joint',
20+
'r_shoulder_pan_joint',
21+
'r_shoulder_lift_joint',
22+
'r_upper_arm_roll_joint',
23+
'r_forearm_roll_joint',
24+
'r_elbow_flex_joint',
25+
'r_wrist_flex_joint',
26+
'r_wrist_roll_joint',
27+
'l_shoulder_pan_joint',
28+
'l_shoulder_lift_joint',
29+
'l_upper_arm_roll_joint',
30+
'l_forearm_roll_joint',
31+
'l_elbow_flex_joint',
32+
'l_wrist_flex_joint',
33+
'l_wrist_roll_joint',
34+
drive_joint_name,
35+
]
36+
),
37+
behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=False, debug_mode=True,
38+
simulation_max_hz=20),
39+
qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT))
40+
giskard.live()

‎scripts/iai_robots/stretch/stretch_standalone.py

+1-1
Original file line numberDiff line numberDiff line change
@@ -12,5 +12,5 @@
1212
giskard = Giskard(world_config=WorldWithDiffDriveRobot(),
1313
collision_avoidance_config=StretchCollisionAvoidanceConfig(),
1414
robot_interface_config=StretchStandaloneInterface(),
15-
behavior_tree_config=StandAloneBTConfig())
15+
behavior_tree_config=StandAloneBTConfig(publish_tf=True, publish_js=True, debug_mode=True))
1616
giskard.live()

‎scripts/iai_robots/stretch/stretch_standalone_vrb.py

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
#!/usr/bin/env python
2+
import rospy
3+
4+
from giskardpy.configs.behavior_tree_config import StandAloneBTConfig
5+
from giskardpy.configs.giskard import Giskard
6+
from giskardpy.configs.iai_robots.stretch import StretchCollisionAvoidanceConfig, StretchStandaloneInterface
7+
from giskardpy.configs.world_config import WorldWithDiffDriveRobot
8+
from giskardpy.configs.qp_controller_config import QPControllerConfig, SupportedQPSolver
9+
from giskardpy.model.ros_msg_visualization import VisualizationMode
10+
11+
if __name__ == '__main__':
12+
rospy.init_node('giskard')
13+
giskard = Giskard(world_config=WorldWithDiffDriveRobot(),
14+
collision_avoidance_config=StretchCollisionAvoidanceConfig(),
15+
robot_interface_config=StretchStandaloneInterface(),
16+
behavior_tree_config=StandAloneBTConfig(publish_js=False, publish_tf=True, simulation_max_hz=20,
17+
debug_mode=True,
18+
visualization_mode=VisualizationMode.CollisionsDecomposedFrameLocked),
19+
qp_controller_config=QPControllerConfig(qp_solver=SupportedQPSolver.qpSWIFT))
20+
giskard.live()

0 commit comments

Comments
 (0)